// GazeTracking.cpp : Defines the entry point for the console application.
//
#include <pxcsession.h>
#include <pxcfacemodule.h>
#include <pxcfacedata.h>
#include <pxcvideomodule.h>
#include "pxcfaceconfiguration.h"
#include "pxcmetadata.h"
#include <pxcsensemanager.h>
#include <pxcfaceconfiguration.h>
#include <pxcprojection.h>

#include <conio.h>
#include <stdio.h>
#include <map>
#include <vector>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/flann/miniflann.hpp>
#include <opencv2\opencv.hpp>
#include "NATUtils.h"
#include <osg/Matrix>
#include <osg/Vec3>
#include <osgDB/ReadFile>
#include <osgUtil/SmoothingVisitor>
#include <osgUtil/LineSegmentIntersector>
#include <LowPass.h>
#include "Utilities.h"

using namespace std;
#define NULL 0

#define IDD_MAINFRAME                   101
#define IDR_MENU                        102
#define IDB_BITMAP_BG                   103
#define IDB_BITMAP_NA                   104
#define IDB_BITMAP_RG                   105
#define ID_STATUS                       10000
#define ID_DEVICE                       20000
#define ID_STREAM                       30000
#define ID_OPTION                       40000
#define ID_MODE_PLAYBACK                40008
#define ID_MODE_LIVE                    40010
#define ID_MODE                         40020
#define ID_COMMAND                      40021
#define ID_COMMAND_START                40030
#define ID_COMMAND_STOP                 40031
#define IDC_MPANEL                      50001
#define IDC_MPANEL1                     50002
#define IDC_MPANEL2                     50003
#define IDC_MPANEL3                     50004

const int MAX_ID = 100;
//const int IDXM_MODE = 2;
static const int NPROFILES_MAX = 100;
static const int NDEVICES_MAX = 100;

static const int IDC_STATUS = 10000;
static const int ID_DEVICEX = 21000;
static const int ID_STREAM1X = 22000;
static const int IDXM_DEVICE = 0;
static const int IDXM_MODE = 1;
static const int IDXM_SYNC = 2;


/* SDK instances */
PXCSession *g_session = 0;
PXCCaptureManager *captureManager = NULL;

PXCCapture *g_capture = NULL;
PXCCapture::Device *g_device = 0;
PXCCaptureManager *g_captureManager = 0;
PXCImage *verticesImage;

FILE* fFile = NULL;
FILE* fLiddown = NULL;
FILE* fCalibration = NULL;
FILE* leftPupilFile = NULL;
FILE* rigthPupilFile = NULL;
FILE* headPosFile = NULL;

FILE* leftPupilFile2 = NULL;
FILE* rigthPupilFile2 = NULL;


/* Menu */
static std::map<int /*ctrl*/, pxcUID /*iuid*/> g_devices;
std::map< int, PXCFaceData::HeadPosition > headPoseMap;
std::map< int, PXCFaceData::PoseEulerAngles	 > headAnglesMap;
std::map<int, PXCPoint3DF32> leftCenterMap;
std::map<int, PXCPoint3DF32> rightCenterMap;
std::map<int, osg::Vec3d> correctedLeftCenterMap;
std::map<int, osg::Vec3d> correctedRightCenterMap;
std::map<int, PXCPoint3DF32> leftPupilCenterMap;
std::map<int, osg::Vec3d> correctedLeftPupilCenterMap;
std::map<int, PXCPoint3DF32> leftEyeLidLeftMap;
std::map<int, PXCPoint3DF32> leftEyeLidRightMap;
std::map<int, osg::Vec3d> correctedleftEyeLidLeftMap;
std::map<int, osg::Vec3d> correctedleftEyeLidRightMap;
std::map<int, double> eyeDiametertMap;

std::map<int, osg::Vec3d> LeftCenterFixMap;
std::map<int, osg::Vec3d> RightCenterFixMap;
std::map<int, osg::Vec3d> LCenterFixMap;
std::map<int, osg::Vec3d> RCenterFixMap;

std::map<int, osg::Vec3d> LeftCenterDeltaMap;
std::map<int, osg::Vec3d> RightCenterDeltaMap;
std::map<int, osg::Vec3d> LCenterDeltaMap;
std::map<int, osg::Vec3d> RCenterDeltaMap;

std::map<int, osg::Vec3d> EYE_LEFT_CENTER_MAP;
std::map<int, osg::Vec3d> EYELID_LEFT_LEFT_MAP;
std::map<int, osg::Vec3d> EYELID_LEFT_RIGHT_MAP;
std::map<int, osg::Vec3d> EYELID_LEFT_BOTTOM_MAP;
std::map<int, osg::Vec3d> EYELID_LEFT_TOP_MAP;

std::map<int, osg::Vec3d> EYE_RIGHT_CENTER_MAP;
std::map<int, osg::Vec3d> EYELID_RIGHT_TOP_MAP;
std::map<int, osg::Vec3d> EYELID_RIGHT_BOTTOM_MAP;
std::map<int, osg::Vec3d> EYELID_RIGHT_LEFT_MAP;
std::map<int, osg::Vec3d> EYELID_RIGHT_RIGHT_MAP;

std::map<int, osg::Vec3d> HEYE_LEFT_CENTER_MAP;
std::map<int, osg::Vec3d> HEYELID_LEFT_LEFT_MAP;
std::map<int, osg::Vec3d> HEYELID_LEFT_RIGHT_MAP;
std::map<int, osg::Vec3d> HEYELID_LEFT_BOTTOM_MAP;
std::map<int, osg::Vec3d> HEYELID_LEFT_TOP_MAP;

std::map<int, osg::Vec3d> HEYE_RIGHT_CENTER_MAP;
std::map<int, osg::Vec3d> HEYELID_RIGHT_TOP_MAP;
std::map<int, osg::Vec3d> HEYELID_RIGHT_BOTTOM_MAP;
std::map<int, osg::Vec3d> HEYELID_RIGHT_LEFT_MAP;
std::map<int, osg::Vec3d> HEYELID_RIGHT_RIGHT_MAP;



std::map<int, osg::Vec3d> ppLDeg;
std::map<int, osg::Vec3d> ppRDeg;
std::map<int, osg::Vec3d> pcRDeg;
std::map<int, osg::Vec3d> pcLDeg;
std::map<int, osg::Vec3d> pCalculatedLeftEyeAngle;

std::map<int, osg::Vec2d> leftPixelMap;
std::map<int, osg::Vec2d> rightPixelMap;
std::map<int, osg::Vec3d> leftEyeAngle;
std::map<int, osg::Vec3d> rightEyeAngle;

//std::map<double, double> leftHeadingMap;
//std::map<double, double> rigthHeadingMap;
//std::map<double, double> leftPitchMap;
//std::map<double, double> rigthPitchMap;
std::map<int, osg::Vec2d> leftHeadingMap;
std::map<int, osg::Vec2d> rigthHeadingMap;
std::map<int, osg::Vec2d> leftPitchMap;
std::map<int, osg::Vec2d> rigthPitchMap;

std::map<int, osg::Vec3d>  ScreenPath;
std::map<int, Conversion> ScreenConversion;
std::map<double, Conversion*> angleResults;
Conversion table[800][600];

Line leftHeading;
Line rightHeading;
Line leftPitch;
Line rightPitch;
Cal leftCal;
Cal rightCal;
FILE* ffN = NULL;

std::map<int, frame> losList;
LowPass pupilFilterL(30);
LowPass pupilFilterR(30);
LowPass pupilCenterFilterL(30);
LowPass pupilCenterFilterR(30);
LowPass headPoseFilter(30);
LowPass headAngleFilter(30);
LowPass aimLFilter(10);
LowPass aimRFilter(10);
LowPass nL(1);
LowPass nR(1);
LowPass lAngle(30);
LowPass rAngle(30);

double ZLavg = 0;
double ZRavg = 0;
double YLavg = 0;
double YRavg = 0;
double XLavg = 0;
double XRavg = 0;
osg::Vec3d leftEyeCenter, rightEyeCenter;
double leftEyeDiameter, rightEyeDiameter;
const double PI = 3.14159265359;

volatile bool g_singleStream = false;
//cv::VideoWriter out_capture("video.avi", CV_FOURCC('M', 'J', 'P', 'G'), 30, cv::Size(800, 600));
cv::VideoWriter out_capture("video.avi", CV_FOURCC('M', 'J', 'P', 'G'), 15, cv::Size(800, 600));
cv::Mat dst = cv::Mat(600, 800, CV_8UC3);
cv::Mat smalColor = cv::Mat(600/3, 800/3, CV_8UC3);
cv::Mat dstTemp = cv::Mat(600, 800, CV_8UC4);
struct {
	int module;
	int device;
	int stream[PXCCapture::STREAM_LIMIT];
	void Clear() {
		for (int i = 0; i<PXCCapture::STREAM_LIMIT; i++) stream[i] = -1;
	}
} g_config;

//osg::ref_ptr<osg::Node> scene;
osg::Node* scene;
vector<PXCPoint3DF32> vertices;
PXCProjection *projection;

//-------------------headers
bool getDepthRectangle(PXCPointF32 point, PXCImage* rgb, PXCImage* depth, double& centerX, double& centerY, double& width, double& height);
bool lineIntersecDir(osg::Vec3d p0, osg::Vec3d dir, double z, osg::Vec3d& intersection);

void calculateAvg(std::map<double, Conversion*> angleResults)
{
	std::map<double, Conversion*>::iterator itr = angleResults.begin();
	for (; itr != angleResults.end(); itr++)
	{
		double  i = itr->first;
		Conversion* p = itr->second;

		if (p->next != NULL)
		{
			int n = 0;
			p->eyeLAvg = osg::Vec2d(0,0);
			p->eyeRAvg = osg::Vec2d(0, 0);
			p->sttL = 0.0;// osg::Vec2d(0, 0);
			p->sttR = 0.0;// osg::Vec2d(0, 0);

			Conversion* ptr = p->next;
			while (ptr != NULL)
			{
				p->eyeLAvg += ptr->eyeL;
				p->eyeRAvg += ptr->eyeR;
				n++;
				ptr = ptr->next;
			}
			p->eyeLAvg /= (double)n;
			p->eyeRAvg /= (double)n;

			ptr = p->next;
			while (ptr != NULL)
			{
				//osg::Vec2d temp = 
				p->sttL += (p->eyeLAvg - ptr->eyeL).length()/n;
				p->sttR += (p->eyeRAvg-ptr->eyeR).length()/n;
				ptr = ptr->next;
			}

			ptr = p->next;
			while (ptr != NULL)
			{
				//osg::Vec2d temp = 
				if ((p->eyeLAvg - ptr->eyeL).length() > 2.0*p->sttL)
				{
					ptr->useIt = false;
				}
				//if ((p->eyeRAvg - ptr->eyeR).length() > 2.0*p->sttR)
				//{
				//	ptr->useIt = false;
				//}

				ptr = ptr->next;
			}

			n = 0;
			p->eyeLAvg = osg::Vec2d(0, 0);
			p->eyeRAvg = osg::Vec2d(0, 0);
			//p->sttL = 0.0;// osg::Vec2d(0, 0);
			//p->sttR = 0.0;// osg::Vec2d(0, 0);

			ptr = p->next;
			while (ptr != NULL)
			{
				if (ptr->useIt)
				{
					p->eyeLAvg += ptr->eyeL;
					p->eyeRAvg += ptr->eyeR;
					n++;
				}
				ptr = ptr->next;
			}
			p->eyeLAvg /= (double)n;
			p->eyeRAvg /= (double)n;
			
		}

	}
}
void findPoint(std::map<double, Conversion*> table, osg::Vec2d left, osg::Vec2d right, osg::Vec2d& pxleft, osg::Vec2d& pxright)
{
	double angle;
	double length = 100000.0;
	std::map<double, Conversion*>::iterator itr = table.begin();
	for (; itr != table.end(); itr++)
	{
		double  i = itr->first;
		Conversion* p = itr->second;
		double t = (p->eyeLAvg - left).length();
		if ( t  < length)
		{
			length = (p->eyeLAvg - left).length();
			angle = i;
		}

	}
	pxleft = table[angle]->pixel;
}
void myPrintF(std::map<double, Conversion*> angleResults,std::string name)
{
	FILE* ff = fopen(name.c_str(), "w");
	std::map<double, Conversion*>::iterator itr = angleResults.begin();
	for (; itr != angleResults.end(); itr++)
	{
		double  i = itr->first;
		Conversion* p = itr->second;
		fprintf(ff,"angle(%lf) avgL(%lf,%lf) sttL(%lf)",i, p->eyeLAvg.x(), p->eyeLAvg.y(), p->sttL);

		if (p->next != NULL)
		{
			Conversion* ptr = p->next;
			while (ptr != NULL)
			{
				fprintf(ff,"-left(%lf,%lf) use(%d)",  ptr->eyeL.x(), ptr->eyeL.y(),(int)ptr->useIt);
				ptr = ptr->next;
			}
			fprintf(ff,"----\n");
		}

	}
	//for each (object var in collection_to_loop)
	//{

	//}
}
void myPrint(std::map<double, Conversion*> angleResults)
{
	std::map<double, Conversion*>::iterator itr = angleResults.begin();
	for (; itr != angleResults.end(); itr++)
	{
		double  i = itr->first;
		Conversion* p = itr->second;
		
		if (p->next != NULL)
		{
			Conversion* ptr = p->next;
			while (ptr != NULL)
			{
				printf("Angle:%lf -Lpix(%lf,%lf)||", i, ptr->eyeL.x(), ptr->eyeL.y());
				ptr = ptr->next;
			}
			printf("\n");
		}

	}
		//for each (object var in collection_to_loop)
		//{

		//}
}



bool ParseCommand()
{
	bool result = false;
	if (kbhit())
	{
		switch (char c = getch())
		{
		case 'q':

			break;

		case 27:
			result = true;
			break;
		}
	}
		return result;
}
//static void ReleaseDevice(void) {
//	if (g_device) g_device->Release();
//	if (g_capture && !g_captureManager) g_capture->Release();
//	g_device = 0;
//	g_capture = 0;
//}
//static void ReleaseDeviceAndCaptureManager(void) {
//	ReleaseDevice();
//	if (g_captureManager)
//	{
//		g_captureManager->CloseStreams();
//		g_captureManager->Release();
//	}
//	g_captureManager = 0;
//}
//
//
//PXCCapture::Device::StreamProfileSet GetProfileSet()
//{
//	PXCCapture::Device::StreamProfileSet profiles = {};
//	if (!g_device) return profiles;
//
//	PXCCapture::DeviceInfo dinfo;
//	g_device->QueryDeviceInfo(&dinfo);
//	for (int s = 0; s<PXCCapture::STREAM_LIMIT; s++) {
//		if (g_config.stream[s]<0) continue;
//		PXCCapture::StreamType st = PXCCapture::StreamTypeFromIndex(s);
//
//		PXCCapture::Device::StreamProfileSet profiles1 = {};
//		g_device->QueryStreamProfileSet(st, g_config.stream[s], &profiles1);
//		profiles[st] = profiles1[st];
//	}
//	return profiles;
//}
//
//PXCCapture::DeviceInfo GetCheckedDevice()
//{
//	PXCCapture::DeviceInfo dinfo = {};
//	if (g_device) g_device->QueryDeviceInfo(&dinfo);
//	return dinfo;
//}
//
//https://software.intel.com/sites/landingpage/realsense/camera-sdk/v1.1/documentation/html/manuals_coordinate_systems.html
/*************************************************************
This function convert the screen pixels to world position
Input: pixel
Output: World (is the 3D camera coordinates system)
**************************************************************/
osg::Vec3d pixelToWorld(osg::Vec2d pixel)
{
	osg::Vec2d	   imageResolution(800.0, 600.0);
	osg::Vec3d worldSize(345.0, -195.0, 0.0);
	osg::Vec2d nPixel;
	osg::Vec3d screenOffset(0, -43, -10);//
	nPixel.x() = -1.0 *(((pixel.x()) / imageResolution.x()) - 0.5);
	nPixel.y() = (pixel.y()) / imageResolution.y();
	osg::Vec3d world;
	world.x() = worldSize.x()*nPixel.x() + screenOffset.x();
	world.y() = worldSize.y()*nPixel.y() + screenOffset.y();
	world.z() = worldSize.z() + screenOffset.z();
	return world;
}
/*************************************************************
This function convert the world position  to screen pixels 
Supose that z coordinates is on the screen.
Input: World (is the 3D camera coordinates system)
Output:pixel 
**************************************************************/
osg::Vec2d worldToPixel(osg::Vec3d world)
{
	osg::Vec2d pixel;
	osg::Vec2d	   pixelW(800.0, 600.0);
	osg::Vec2d w(345, -195);
	osg::Vec2d offset(-10 + 345 / 2.0, -43);
	world.x() = 1.0 - ((world.x() + offset.x()) / w.x());
	world.y() = (world.y() - offset.y()) / w.y();
	pixel.x() = world.x() * pixelW.x();
	pixel.y() = world.y() * pixelW.y();
	return pixel;
}
/*************************************************************
This function Capture an IR image and convert in to Opencv.
The Image can be showed or not.

Input:
PXCImage* irImage:  sample to ir image
bool draw		 : 	if draw the image

Output			:  Opencv Mat (Image)
**************************************************************/
cv::Mat drawIrImage(PXCImage* irImage, bool draw)
{
	PXCImage::ImageInfo info = irImage->QueryInfo();
	cv::Mat cvImage;
	PXCImage::ImageData data;
	if (irImage->AcquireAccess(PXCImage::ACCESS_READ, PXCImage::PIXEL_FORMAT_Y8, &data) >= PXC_STATUS_NO_ERROR)
	{
		cvImage = cv::Mat(info.height, info.width, CV_8UC1, data.planes[0], cv::Mat::AUTO_STEP);
		if (draw)
		{
			cv::imshow("IR face", cvImage);
			cv::waitKey(1);
		}
		irImage->ReleaseAccess(&data);
	}
	return cvImage;
}
/*************************************************************
This function Capture an IR image show with adding a marker .
Input:
PXCImage* irImage:  sample to ir image
PXCPointF32 point : marker position(pixel)

Output			:  void
**************************************************************/
void drawIrImage(PXCImage* irImage, PXCPointF32 point)
{
	cv::Mat cvImage = drawIrImage(irImage, false);

	cv::circle(cvImage, cv::Point(point.x, point.y), 2, cv::Scalar(255, 255, 255));
	cv::imshow("IR Image", cvImage);
	cv::waitKey(1);
}
/*************************************************************
This function Capture an Depth image and convert in to Opencv.
The Image can be showed or not.

Input:
PXCImage* depthImage:  sample to depth image
bool draw			: 	if draw the image

Output			:  Opencv Mat (Image)
**************************************************************/
cv::Mat drawDepthImage(PXCImage* depthImage, bool draw)
{
	PXCImage::ImageInfo info = depthImage->QueryInfo();
	cv::Mat cvImage;
	PXCImage::ImageData data;
	if (depthImage->AcquireAccess(PXCImage::ACCESS_READ, PXCImage::PIXEL_FORMAT_DEPTH_F32, &data) >= PXC_STATUS_NO_ERROR)
	{
		cvImage = cv::Mat(info.height, info.width, CV_32F, data.planes[0], cv::Mat::AUTO_STEP);
		if (draw)
		{
			cv::imshow("Depth face", cvImage);
			cv::waitKey(1);
		}
		depthImage->ReleaseAccess(&data);
	}
	return cvImage;
}
/*************************************************************
This function Capture an Depth image show with adding a marker .
Input:
PXCImage* depthImage:  sample to depth image
PXCPointF32 point : marker position(NORMALIZED pixel)

Output			:  void
**************************************************************/
void drawDepthImage(PXCImage* depthImage,PXCPointF32 point)	//Normalized points
{
	PXCImage::ImageInfo info = depthImage->QueryInfo();
	cv::Mat cvImage = drawDepthImage(depthImage, false);

	double minVal, maxVal;
	cv::Mat draw;
	cv::minMaxLoc(cvImage, &minVal, &maxVal);
	cvImage.convertTo(draw, CV_8UC3, 255.0 / (maxVal - minVal), -minVal * 255.0 / (maxVal - minVal));

	cv::circle(draw, cv::Point(point.x*info.width, (point.y)*info.height), 2, cv::Scalar(255, 0, 0));
	cv::imshow("Depth face", draw);
	cv::waitKey(1);
}
/*************************************************************
This function Normalize a pixel relative to some ROI .
Input:
double left :	 ROI left
double top	:	 ROI top
int width	:	 ROI width
int height	:	 ROI height
float x		:	 pixel X
float y		:	 pixel Y

Output		
PXCPointF32& point:  normalized pixel
return bool: false if the point is NOT inside the ROI
**************************************************************/
bool NormalizePoint(double left, double top, int width, int height, float x, float y, PXCPointF32& point)
{


	// normalize coordinates

	//PXCPointF32 point;
	point.x = double(x - (left + 0.0)) / (double)(width);// +1.0);
	point.y = double(y - (top + 0.0)) / (double)(height);// +1.0);
	// check values
	if (point.x <= 0.f || point.x >= 1.f || point.y <= 0.f || point.y >= 1.f) {
		return false;
	}
	return true;
}
/*************************************************************
This function Convert Color Image Coordinate into Depth Image Coordinate .
Input:
PXCImage *color				:  colorImage
PXCImage *depth				:  depthImage
vector<PXCPointF32> ccords	:  vector of color coordinates

Output
vector<PXCPointF32> &dcords:  vector of converted depth coordinates
return : void
**************************************************************/
void ColorToDepth(PXCImage *color, PXCImage *depth, vector<PXCPointF32> ccords, vector<PXCPointF32> &dcords)
{
	if (!projection) return;
	int csize = ccords.size();
	PXCPointF32 invP = { -1.f, -1.f };
	vector<PXCPointF32> ccordsAbs(csize, invP);
	float cwidth = (float)color->QueryInfo().width, cheight = (float)color->QueryInfo().height;
	for (int i = 0; i < csize; i++) {
		ccordsAbs[i].x = ccords[i].x * cwidth, ccordsAbs[i].y = ccords[i].y * cheight;
	}
	dcords.resize(csize);
	projection->MapColorToDepth(depth, csize, &ccordsAbs[0], &dcords[0]);
	float dwidthInv = 1.f / depth->QueryInfo().width, dheightInv = 1.f / depth->QueryInfo().height;
	for (int i = 0; i < csize; i++) {
		if (dcords[i].x < 0) continue;
		dcords[i].x *= dwidthInv; dcords[i].y *= dheightInv;
	}
}
/*************************************************************
This function Convert Depth Image Coordinate into  Color Image Coordinate .
Input:
PXCImage *color				:  colorImage
PXCImage *depth				:  depthImage
vector<PXCPointF32> dcords	:  vector of depth coordinates

Output
vector<PXCPointF32> &ccords:  vector of converted color coordinates
return : void
**************************************************************/
void DepthToColor(PXCImage *color, PXCImage *depth, vector<PXCPointF32> dcords, vector<PXCPointF32> &ccords)
{
	PXCImage::ImageData ddata;
	if (!projection) return;
	if (PXC_STATUS_NO_ERROR > depth->AcquireAccess(PXCImage::ACCESS_READ, PXCImage::PIXEL_FORMAT_DEPTH, &ddata))
		return;
	int dsize = dcords.size();
	PXCPoint3DF32 invP3D = { -1.f, -1.f, 0.f };
	vector<PXCPoint3DF32> dcordsAbs(dsize, invP3D);
	PXCPointF32 invP = { -1.f, -1.f };
	ccords.resize(dsize, invP);
	float dwidth = (float)depth->QueryInfo().width, dheight = (float)depth->QueryInfo().height;
	for (int i = 0; i < dsize; i++) {
		if (dcords[i].x < 0) continue;
		dcordsAbs[i].x = dcords[i].x * dwidth, dcordsAbs[i].y = dcords[i].y * dheight;
		dcordsAbs[i].z = (float)(((pxcI16*)(ddata.planes[0] + (int)dcordsAbs[i].y * ddata.pitches[0]))[(int)dcordsAbs[i].x]);
	}
	depth->ReleaseAccess(&ddata);

	projection->MapDepthToColor(dsize, &dcordsAbs[0], &ccords[0]);
	float cwidthInv = 1.f / color->QueryInfo().width, cheightInv = 1.f / color->QueryInfo().height;
	for (int i = 0; i < dsize; i++) {
		if (ccords[i].x < 0) ccords[i].x = ccords[i].y = -1.f;
		else ccords[i].x *= cwidthInv; ccords[i].y *= cheightInv;
	}
}
/*************************************************************
This function Draw the image and add head position markers.
Input:
std::string title	:	windows title
cv::Mat& colorImage	:	image to be showed
 osg::Vec3d headPos	:	head pos
 osg::Vec3d headDeg	:	head orientation

Output
return : void
**************************************************************/
void DrawHead(std::string title,cv::Mat& colorImage, osg::Vec3d headPos, osg::Vec3d headDeg)
{
	cv::Size size = colorImage.size();

	cv::Point p1(size.width / 2, 230);
	cv::Point p2(size.width / 2, 350);
	cv::Point p3(150 + size.width / 2, 230);
	cv::Point p4(150 + size.width / 2, 350);
	cv::Point centerY(10.0*headDeg.x() + size.width / 2, 250);
	cv::Point centerR(10.0*headDeg.y() + size.width / 2, 250);
	cv::Point centerP(10.0*headDeg.z() + size.width / 2, 250);
	cv::Rect xH(-headPos.x()*10.0  + size.width / 2, 250 + 40, 20, 20);
	//printf("Head Angle(%f,%f)\n", angles.yaw, angles.roll);
	cv::line(colorImage, p1, p2, cv::Scalar(0, 0, 255), 1);
	cv::line(colorImage, p3, p4, cv::Scalar(0, 0, 255), 1);
	cv::circle(colorImage, centerY, 10, cv::Scalar(0, 255, 255), 1);
	cv::circle(colorImage, centerR, 10, cv::Scalar(0, 255, 128), 1);
	cv::circle(colorImage, centerP, 10, cv::Scalar(255, 0, 128), 1);
	cv::rectangle(colorImage, xH, cv::Scalar(255, 0, 0), 1);
	cv::namedWindow(title, CV_WINDOW_NORMAL);
	cv::setWindowProperty(title, CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
	cv::imshow(title, colorImage);
	cv::waitKey(1);
}
/*************************************************************
This function Capture an Color image and convert in to Opencv.
The Image can be showed or not.

Input:
PXCImage* colorImage:  sample to color image
bool draw			: 	if draw the image

Output			:  Opencv Mat (Image)
**************************************************************/
cv::Mat drawColorImage(PXCImage* colorImage, bool draw)
{
	PXCImage::ImageInfo info = colorImage->QueryInfo();
	cv::Mat cvImage;
	PXCImage::ImageData data;
	if (colorImage->AcquireAccess(PXCImage::ACCESS_READ, PXCImage::PIXEL_FORMAT_RGB32, &data) >= PXC_STATUS_NO_ERROR)
	{

		cvImage = cv::Mat(info.height, info.width, CV_8UC4, data.planes[0], cv::Mat::AUTO_STEP);

		if (draw)
		{
			cv::imshow("ColorImage", cvImage);
			cv::waitKey(1);
		}

		colorImage->ReleaseAccess(&data);
	}
	return cvImage;
}
void findEyes2(PXCFaceData::Face* trackedFace, cv::Mat& rgb)
{
	const PXCFaceData::LandmarksData* landmarkData = trackedFace->QueryLandmarks();
	int i;
	//Left eye image
	PXCFaceData::LandmarkPoint leftCenter;
	i = landmarkData->QueryPointIndex(PXCFaceData::LANDMARK_EYE_LEFT_CENTER);
	landmarkData->QueryPoint(i, &leftCenter);
	//Right eye image
	PXCFaceData::LandmarkPoint rightCenter;
	i = landmarkData->QueryPointIndex(PXCFaceData::LANDMARK_EYE_RIGHT_CENTER);
	landmarkData->QueryPoint(i, &rightCenter);
	cv::Point cl =cv::Point(leftCenter.image.x, leftCenter.image.y);
	cv::Point cr = cv::Point(rightCenter.image.x, rightCenter.image.y);
	cv::circle(rgb, cl, 1, cv::Scalar(0, 255, 255), -1, 8, 0);
	cv::circle(rgb, cr, 1, cv::Scalar(0, 255, 255), -1, 8, 0);

}
void findEyes(PXCFaceData::Face* trackedFace, PXCImage* rgb, cv::Rect& LeftleftDown, cv::Rect& RightleftDown, osg::Vec2d& ll, osg::Vec2d& lr)
{
	const PXCFaceData::LandmarksData* landmarkData = trackedFace->QueryLandmarks();

	int i;

	//Left eye image
		PXCFaceData::LandmarkPoint leftLeft, leftRight, leftBottom,leftUP;
		i = landmarkData->QueryPointIndex(PXCFaceData::LANDMARK_EYELID_LEFT_LEFT);
		landmarkData->QueryPoint(i, &leftLeft);
		ll = osg::Vec2d(leftLeft.image.x, leftLeft.image.y);
		i = landmarkData->QueryPointIndex(PXCFaceData::LANDMARK_EYELID_LEFT_RIGHT);
		landmarkData->QueryPoint(i, &leftRight);
		lr = osg::Vec2d(leftRight.image.x, leftRight.image.y);

		i = landmarkData->QueryPointIndex(PXCFaceData::LANDMARK_EYELID_LEFT_BOTTOM);
		landmarkData->QueryPoint(i, &leftBottom);

		i = landmarkData->QueryPointIndex(PXCFaceData::LANDMARK_EYEBROW_LEFT_CENTER);
		landmarkData->QueryPoint(i, &leftUP);

		cv::Rect Lld;
		Lld.x = leftRight.image.x - 00;
		Lld.y = leftUP.image.y+30;//check if not max....(like windows	
		Lld.width = (leftLeft.image.x + 00) - Lld.x;
		Lld.height = (min(min(leftLeft.image.y, leftRight.image.y), leftBottom.image.y) + 30) - Lld.y;
		LeftleftDown = Lld;


		//Right eye image
		PXCFaceData::LandmarkPoint rightLeft, rightRight, rightBottom, rightUP;
		i = landmarkData->QueryPointIndex(PXCFaceData::LANDMARK_EYELID_RIGHT_LEFT);
		landmarkData->QueryPoint(i, &rightLeft);
		i = landmarkData->QueryPointIndex(PXCFaceData::LANDMARK_EYELID_RIGHT_RIGHT);
		landmarkData->QueryPoint(i, &rightRight);

		i = landmarkData->QueryPointIndex(PXCFaceData::LANDMARK_EYELID_RIGHT_BOTTOM);
		landmarkData->QueryPoint(i, &rightBottom);

		i = landmarkData->QueryPointIndex(PXCFaceData::LANDMARK_EYEBROW_RIGHT_CENTER);
		landmarkData->QueryPoint(i, &rightUP);

		cv::Rect Rld;
		Rld.x = rightRight.image.x - 00;
		Rld.y = rightUP.image.y+30;//check if not max....(like windows	   rightLeft
		Rld.width = (rightLeft.image.x + 00) - Rld.x;
		Rld.height = (min(min(rightLeft.image.y, rightRight.image.y), rightBottom.image.y) + 30 )- Rld.y;

		RightleftDown = Rld;

}
/*************************************************************
This function Detect Circles in an image by  Hough Transform. 

Input:
cv::Mat& color		:	 image
int blur			:	 blur kernel size
int minRadius		:	 min radius
int maxRadius		:	 max radius
int minThreshold	:	 min threshold

Output			:  
vector<cv::Vec3f> circles	: vector of circles

**************************************************************/
vector<cv::Vec3f> detectCircles(cv::Mat& color, int blur, int minRadius, int maxRadius, int minThreshold)
{
	cv::Mat src_gray;
	cv::cvtColor(color, src_gray, CV_BGR2GRAY);

	cv::Mat img_hist_eq;
	cv::equalizeHist(src_gray, img_hist_eq);
	//img_hist_eq = img_hist_eq * 5;

	cv::Mat mask;
	//threshold(img_hist_eq, mask, 16, 255, CV_THRESH_BINARY);

	//img_hist_eq = img_hist_eq | mask;

	//cv::imshow("img_hist_eq", img_hist_eq);
	//cv::waitKey(1);
	///// Reduce the noise so we avoid false circle detection
	////cv::GaussianBlur(src_gray, src_gray, cv::Size(5, 5), 2, 2);

	vector<cv::Vec3f> circles;

	if (blur > 2)
	{
		cv::GaussianBlur(img_hist_eq, img_hist_eq, cv::Size(blur, blur), 2, 2);
	}
	cv::HoughCircles(img_hist_eq, circles, CV_HOUGH_GRADIENT, 1, src_gray.rows / 2, 200, minThreshold, minRadius, maxRadius);	/// Draw the circles detected
	//for (size_t i = 0; i < circles.size(); i++)
	//{
	//	cv::Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
	//	int radius = cvRound(circles[i][2]);
	//	// circle center
	//	cv::circle(color, center, 1, cv::Scalar(0, 255, 0), -1, 8, 0);
	//	// circle outline
	//	circle(color, center, radius, cv::Scalar(0, 0, 255), 1, 8, 0);
	//}
	return circles;
}

/*************************************************************
This function Detect Circles in an image by  Hough Transform.
  This function find by imself the parameters to obtain a good result
Input:
cv::Mat color		:	 image

Output			:
vector<cv::Vec3f> circles	: vector of circles

**************************************************************/
bool detectCircles(cv::Mat color, vector<cv::Vec3f>& circles)
{
	int r =  color.size().width / 4;
	int threshold = 5;
	//detectCircles(color, 5, r *0.9, r *1.10, 5);
	circles.clear();

	float radiusFactor = 0.1;
	//vector<cv::Vec3f> circles;
	int hit = 100;
	while (hit != 1 && radiusFactor <1)
	{
		while (hit > 1)
		{
			threshold++;
			circles = detectCircles(color, 19, r *(1 - radiusFactor), r *(1 + radiusFactor), threshold);
			hit = circles.size();
			
			//if (circles.size() == 1)
			//{
			//	for (size_t i = 0; i < circles.size(); i++)
			//	{
			//		cv::Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
			//		int radius = cvRound(circles[i][2]);
			//		// circle center
			//		cv::circle(color, center, 1, cv::Scalar(0, 255, 0), -1, 8, 0);
			//		// circle outline
			//		circle(color, center, radius, cv::Scalar(0, 0, 255), 1, 8, 0);
			//	}
			//}
			//cv::imshow("ColorImage", color);													 
			//cv::waitKey(1);											  n
		}
		if (hit != 1)
		{
			radiusFactor += 0.0125;
			//threshold = 0;
			hit = 100;
		}
	}
	return true;
}
bool detectCircles(cv::Mat color)
{
	int r = color.size().width / 4;
	int threshold = 0;
	//detectCircles(color, 5, r *0.9, r *1.10, 5);

	float radiusFactor = 0.1;
	vector<cv::Vec3f> circles;
	int hit = 100;
	while (hit != 1 && radiusFactor <1)
	{
		while (hit > 2)
		{
			threshold++;
			circles = detectCircles(color, 0, r *(1 - radiusFactor), r *(1 + radiusFactor), threshold);
			hit = circles.size();
			//if (circles.size() == 1)
			//{
			//	for (size_t i = 0; i < circles.size(); i++)
			//	{
			//		cv::Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
			//		int radius = cvRound(circles[i][2]);
			//		// circle center
			//		cv::circle(color, center, 1, cv::Scalar(0, 255, 0), -1, 8, 0);
			//		// circle outline
			//		circle(color, center, radius, cv::Scalar(0, 0, 255), 1, 8, 0);
			//	}
			//}
			//cv::imshow("ColorImage", color);
			//cv::waitKey(1);
		}
		if (hit != 1)
		{
			radiusFactor += 0.025;
			hit = 100;
		}
	}
	return true;
}

/*************************************************************
This function Capture an Color image show with adding a marker .
Input:
PXCImage* colorImage:  sample to depth image
PXCPointF32 point : marker position( pixel)

Output			:  void
**************************************************************/
void drawColorImage(PXCImage* colorImage, PXCPointF32 point)
{
	cv::Mat cvImage = drawColorImage(colorImage, false);

	detectCircles(cvImage);	  //TODO resolve what to do width this line or open a new function
	cv::circle(cvImage, cv::Point(point.x, point.y), 1, cv::Scalar(255, 255, 255));		
	cv::imshow("ColorImage", cvImage);	
	cv::waitKey(1);
}
// TODO: comment this
bool detectIris(cv::Mat color, osg::Vec2d left, osg::Vec2d right)
{
	cv::Mat src_gray;
	cv::cvtColor(color, src_gray, CV_BGR2GRAY);

	cv::Mat img_hist_eq;
	//cv::equalizeHist(src_gray, img_hist_eq);
	osg::Vec2d dir(right - left);
	cv::Vec3b pixelR(0, 0, 255);
	int height =src_gray.size().height;
	for (float i = 0; i < 1; i += 0.1)
	{
		//cv::Vec3b pixel = img_hist_eq.at<cv::Vec3b>(cv::Point(left.x() + dir.x()*i, left.y() + dir.y()*i));
		//printf("(%d,%d)-(%f,%f)\n", left.x() + dir.x()*i, left.y() + dir.y()*i);
		int x = left.x() + dir.x()*i;
		int y = left.y() + dir.y()*i;

		uchar pixel = src_gray.at<uchar>(cv::Point(x, height-y));
		printf("(%d)-(%d,%d)\n",pixel,x, y);
		//color.at<cv::Vec3b>(cv::Point(x, y)) = pixelR;
		if (pixel > 64)
		{
			cv::circle(color, cv::Point(x, y), 1, cv::Scalar(0, 255, 0), -1, 8, 0);
		}
		else
		{
			cv::circle(color, cv::Point(x, y), 1, cv::Scalar(0, 0, 255), -1, 8, 0);
		}

	}
	cv::imshow("gray", src_gray);
	cv::waitKey(1);
	return true;
	cv::imshow("LineleftImage", color);
	cv::waitKey(1);
	return true;
}
/*************************************************************
This function Detect and draw the Eyes .
Input:
PXCImage* colorImage			:	sample to color image
PXCFaceData::Face* trackedFace	:	trackedFace

Output			:  void
**************************************************************/
void drawEyes(PXCImage* colorImage, PXCFaceData::Face* trackedFace)
{
	vector<cv::Vec3f> circles;
	cv::Mat rgb = drawColorImage(colorImage, false);
	cv::Rect left, right;
	osg::Vec2d ll, lr;
	findEyes(trackedFace, colorImage, left,right,ll,lr);
	findEyes2(trackedFace, rgb);
	//detectIris(rgb, lr, ll);
	cv::Mat leftImage ;
	rgb(left).copyTo(leftImage);
	cv::flip(leftImage, leftImage, 1);
	
	
	cv::Mat rightImage;// = rgb(right);
	rgb(right).copyTo(rightImage);
	cv::flip(rightImage, rightImage, 1);
	detectCircles(leftImage, circles);
	if (circles.size() == 1)
	{
		for (size_t i = 0; i < circles.size(); i++)
		{
			cv::Point center(cvRound(left.x+(left.width-circles[i][0])), cvRound(left.y+circles[i][1]));
			//cv::Vec3b intensity = rgb.at<cv::Vec3b>(center);
			//uchar blue = intensity.val[0];
			//uchar green = intensity.val[1];
			//uchar red = intensity.val[2];
			//if ((blue + green + red) / 3 < 64)
			//{
				int radius = cvRound(circles[i][2]);
				// circle center
				cv::circle(rgb, center, 1, cv::Scalar(0, 255, 0), -1, 8, 0);
				// circle outline
				cv::circle(rgb, center, radius, cv::Scalar(0, 0, 255), 1, 8, 0);
			//}
			//else
			//	cv::circle(rgb, center, 1, cv::Scalar(255, 255, 255), -1, 8, 0);
		}
	}

	//cv::imshow("leftImage", rgb);
	//cv::waitKey(1);
	detectCircles(rightImage, circles);
	if (circles.size() == 1)
	{
		for (size_t i = 0; i < circles.size(); i++)
		{
			//cv::Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
			cv::Point center(cvRound(right.x + (right.width - circles[i][0])), cvRound(right.y + circles[i][1]));
			//cv::Vec3b intensity = rightImage.at<cv::Vec3b>(center);
			//uchar blue = intensity.val[0];
			//uchar green = intensity.val[1];
			//uchar red = intensity.val[2];
			//if ((blue + green + red) / 3 < 64)
			//{
				int radius = cvRound(circles[i][2]);
				// circle center
				cv::circle(rgb, center, 1, cv::Scalar(0, 255, 0), -1, 8, 0);
				// circle outline
				cv::circle(rgb, center, radius, cv::Scalar(0, 0, 255), 1, 8, 0);
			//}
			//else
			//	cv::circle(rightImage, center, 1, cv::Scalar(255, 255, 255), -1, 8, 0);
		}
	}
	cv::imshow("rightImage", rgb);
	cv::waitKey(1);
}

/*************************************************************
This function Test Hough transform in a static Image.
Input			:  void
Output			:  void
**************************************************************/
void TestHough()
{
	cv::Mat src,src2;

	/// Read the image
	src = cv::imread("leftEye.jpg",1);
	while (true)
	{
		src.copyTo(src2);
		detectCircles(src2);
		
		cv::imshow("ColorImage", src2);
		cv::waitKey(1);
	}
}
/*************************************************************
This function Create   Depth vertex .
Input:
PXCImage* depthImage			:	sample to depth image
Output			:  void
**************************************************************/
void createVerticesFromDepth(PXCImage* depthImage)
{
	PXCImage::ImageInfo dinfo = depthImage->QueryInfo();
	pxcStatus sts = projection->QueryVertices(depthImage, &vertices[0]);
	if (sts >= PXC_STATUS_NO_ERROR) {
	}
}
/*************************************************************
This function Draw the Depth vertex .
Input:
PXCImage* depthImage			:	sample to depth image
Output			:  void
**************************************************************/
void drawVertex(PXCImage* depthImage)
{
   //verticesImage
	PXCImage::ImageInfo drawVerticesInfo = verticesImage->QueryInfo();
	PXCImage::ImageData drawVerticesDat;
	if (PXC_STATUS_NO_ERROR > verticesImage->AcquireAccess(PXCImage::ACCESS_WRITE, drawVerticesInfo.format, &drawVerticesDat))
		return ;
	PXCImage::ImageInfo dinfo = depthImage->QueryInfo();
	pxcBYTE* pdrawVerticesDat = drawVerticesDat.planes[0];
	for (pxcI32 y = 1; y < dinfo.height - 1; y++) {
		for (pxcI32 x = 1; x < dinfo.width - 1; x++) {

			pdrawVerticesDat[4 * x + 0] = vertices[y*dinfo.width + x].x;
			pdrawVerticesDat[4 * x + 1] = vertices[y*dinfo.width + x].y;
			pdrawVerticesDat[4 * x + 2] = vertices[y*dinfo.width + x].z;
			pdrawVerticesDat[4 * x + 3] = 255;
		}
		pdrawVerticesDat += drawVerticesDat.pitches[0];
	}

	
	cv::Mat cvImage = cv::Mat(dinfo.height, dinfo.width, CV_8UC4, drawVerticesDat.planes[0], cv::Mat::AUTO_STEP);
	

	//cvIrImage = cvImage;

	cv::imshow("vertices Image", cvImage);
	cv::waitKey(1);

	verticesImage->ReleaseAccess(&drawVerticesDat);

}
/*************************************************************
This function Create VertexImage .
Input:
int width		:	image width
int height		:	image height
Output			:  void
**************************************************************/
void createVertexImage(int width, int height)
{
	PXCImage::ImageInfo drawVerticesInfo;
	memset(&drawVerticesInfo, 0, sizeof(drawVerticesInfo));
	drawVerticesInfo.width = width;
	drawVerticesInfo.height = height;
	drawVerticesInfo.format = PXCImage::PIXEL_FORMAT_RGB32;
	verticesImage = 0;
	verticesImage = g_session->CreateImage(&drawVerticesInfo);
	vertices.resize(width * height);
}
/*RealSense SDK Required Initialization*/
void ReleaseDevice(void)
{
	if (g_device && !g_captureManager)  { g_device->Release();  g_device = 0; }
	if (g_capture) { g_capture->Release(); g_capture = 0; }
}
void SetDevice( int ctrl)
{
	if (!g_captureManager) {
		ReleaseDevice();
		if (g_session->CreateImpl<PXCCapture>(g_devices[ctrl], &g_capture)<PXC_STATUS_NO_ERROR) return;
	}
	g_device = g_capture->CreateDevice(g_config.device);
	if (!g_device) ReleaseDevice();
}
void PopulateDevice()
{


	PXCSession::ImplDesc desc;
	memset(&desc, 0, sizeof(desc));
	desc.group = PXCSession::IMPL_GROUP_SENSOR;
	desc.subgroup = PXCSession::IMPL_SUBGROUP_VIDEO_CAPTURE;

	int defaultDevice = -1;

	for (int i = 0, k = ID_DEVICEX;; ++i)
	{
		PXCSession::ImplDesc desc1;
		if (g_session->QueryImpl(&desc, i, &desc1) < PXC_STATUS_NO_ERROR)
			break;

		PXCCapture *capture;
		if (g_session->CreateImpl<PXCCapture>(&desc1, &capture) < PXC_STATUS_NO_ERROR)
			continue;

		for (int j = 0;; ++j) {
			PXCCapture::DeviceInfo deviceInfo;
			if (capture->QueryDeviceInfo(j, &deviceInfo) < PXC_STATUS_NO_ERROR)
				break;
			int id = ID_DEVICE + i *MAX_ID + j;
			if (defaultDevice<0) defaultDevice = id;
			g_devices[id] = desc1.iuid;
			
			if (g_config.module<0) {
				// apply first device
				g_config.module = i;
				g_config.device = j;
			}

			//SetDevice(defaultDevice);
		}

		capture->Release();
	}
	SetDevice(defaultDevice);
}
void PopulateStreams()
{
	
	g_config.Clear();



	if (g_device) {
		PXCCapture::DeviceInfo dinfo;
		g_device->QueryDeviceInfo(&dinfo);

		for (int s = 0; s<2; s++) {
			PXCCapture::StreamType st = PXCCapture::StreamTypeFromIndex(s);
			if (!(dinfo.streams&st)) break;

			/* Create a profile menu */
			
			int id = ID_STREAM + s*MAX_ID;
			int nprofiles = g_device->QueryStreamProfileSetNum(st);
			int p = 0;
			int defaultProfile = -1;
			for (p = 0; p<nprofiles; p++) {
				PXCCapture::Device::StreamProfileSet profiles = {};
				if (g_device->QueryStreamProfileSet(st, p, &profiles) < PXC_STATUS_NO_ERROR) break;
				PXCCapture::Device::StreamProfile &profile = profiles[st];
				if (defaultProfile<0 && profile.imageInfo.width == 640) defaultProfile = p;
				
			}
			if (p) {
				defaultProfile = max(0, defaultProfile);
				g_config.stream[s] = defaultProfile;
			}
		}
	}


}
/******************************************/
/* linear and bilinear Interpolation , used to calculate normals*/
PXCPoint3DF32 linearInterpolation(double alfa, PXCPoint3DF32 p1, PXCPoint3DF32 p2)
{
	PXCPoint3DF32 x0;
	x0.x = alfa * p1.x + (1.0 - alfa) * p2.x;
	x0.y = alfa * p1.y + (1.0 - alfa) * p2.y;
	x0.z = alfa * p1.z + (1.0 - alfa) * p2.z;
	return x0;
}
osg::Vec3d linearInterpolation(double alfa, osg::Vec3d p1, osg::Vec3d p2)
{
	PXCPoint3DF32 _p1, _p2, r;
	osg::Vec3d res;
	_p1.x = p1.x();
	_p1.y = p1.y();
	_p1.z = p1.z();

	_p2.x = p2.x();
	_p2.y = p2.y();
	_p2.z = p2.z();
	r = linearInterpolation(alfa, _p1, _p2);
	res = osg::Vec3d(r.x, r.y, r.z);
	return res;
}

///      p3.....p4
///      .      .
///      p1.....p2
PXCPoint3DF32 bilinearInterpolation(double alfaX, double alfaY, PXCPoint3DF32 p1, PXCPoint3DF32 p2, PXCPoint3DF32 p3, PXCPoint3DF32 p4)
{
	PXCPoint3DF32 x0, x1, n;
	//x0.x = alfaX * p1.x + (1.0 - alfaX) * p2.x;
	//x0.y = alfaX * p1.y + (1.0 - alfaX) * p2.y;
	//x0.z = alfaX * p1.z + (1.0 - alfaX) * p2.z;
	x0 = linearInterpolation(alfaX, p1, p2);

	//x1.x = alfaX * p3.x + (1.0 - alfaX) * p4.x;
	//x1.y = alfaX * p3.y + (1.0 - alfaX) * p4.y;
	//x1.z = alfaX * p3.z + (1.0 - alfaX) * p4.z;
	x1 = linearInterpolation(alfaX, p3, p4);

	//n.x = alfaY * x0.x + (1.0 - alfaY) * x1.x;
	//n.y = alfaY * x0.y + (1.0 - alfaY) * x1.y;
	//n.z = alfaY * x0.z + (1.0 - alfaY) * x1.z;
	n = linearInterpolation(alfaY, x0, x1);
	return n;
}
osg::Vec3d bilinearInterpolation(double alfaX, double alfaY, osg::Vec3d p1, osg::Vec3d p2, osg::Vec3d p3, osg::Vec3d p4)
{
	PXCPoint3DF32 _p1,_p2,_p3,_p4,r;
	osg::Vec3d res;
	_p1.x = p1.x();
	_p1.y = p1.y();
	_p1.z = p1.z();

	_p2.x = p2.x();
	_p2.y = p2.y();
	_p2.z = p2.z();

	_p3.x = p3.x();
	_p3.y = p3.y();
	_p3.z = p3.z();

	_p4.x = p4.x();
	_p4.y = p4.y();
	_p4.z = p4.z();
	r = bilinearInterpolation(alfaX, alfaY, _p1, _p2, _p3, _p4);
	res = osg::Vec3d(r.x, r.y, r.z);
	return res;
}

void InterpolateVertex(PXCPointF32 point, PXCPoint3DF32& world)
{
	int x0, x1;
	int y0, y1;
	float alfaX, betaX;
	float alfaY, betaY;
	x0 = floor(point.x);
	betaX = point.x - x0;
	alfaX = 1.0 - betaX;
	if (alfaX == 1.0)
		x1 = x0;
	else
		x1 = x0 + 1;

	y0 = floor(point.y);
	betaY = point.y - y0;
	alfaY = 1.0 - betaY;
	if (alfaY == 1.0)
		y1 = y0;
	else
		y1 = y0 + 1;
	if (((y0 * 640 + x0) >= 0) && ((y1 * 640 + x1) >= 0))
	{
		///      p3.....p4
		///      .      .
		///      p1.....p2
		PXCPoint3DF32 vertex1 = vertices[y0 * 640 + x0];
		PXCPoint3DF32 vertex2 = vertices[y0 * 640 + x1];
		PXCPoint3DF32 vertex3 = vertices[y1 * 640 + x0];
		PXCPoint3DF32 vertex4 = vertices[y1 * 640 + x1];
		//world.x = vertex0.x * alfaX + vertex1.x *betaX;
		//world.y = vertex0.y * alfaY + vertex1.y *betaY;
		//world.z = vertex0.z* 0.5 + vertex1.z *0.5;
		//world = bilinearInterpolation(alfaX, alfaY, vertex1, vertex2, vertex3, vertex4);
		world = bilinearInterpolation(1.0, 1.0, vertex1, vertex2, vertex3, vertex4);

	}
}
/* cross and dot product , used to calculate normals */
osg::Vec3d cross(osg::Vec3d v1, osg::Vec3d v2)
{
	osg::Vec3d n1;
	n1.x() = (v1.y() * v2.z()) - (v1.z() * v2.y());
	n1.y() = -((v2.z() * v1.x()) - (v2.x() * v1.z()));
	n1.z() = (v1.x() * v2.y()) - (v1.y() * v2.x());
	return n1;
}
osg::Vec3d dot(osg::Vec3d v1, osg::Vec3d v2)
{
	osg::Vec3d n1;
	n1.x() = v1.x() * v2.x();
	n1.y() = v1.y() * v2.y();
	n1.z() = v1.z() * v2.z();
	return n1;
}
PXCPoint3DF32 CalculateNormal(PXCPoint3DF32 c, PXCPoint3DF32 a, PXCPoint3DF32 b)
{
	osg::Vec3d v1;
	osg::Vec3d v2;
	osg::Vec3d n1;
	double dx, dy, dz;
	//  Vector 1 = Vertex B - Vertex A
	//	Vector 2 = Vertex C - Vertex A
	dx = b.x - a.x;
	dy = b.y - a.y;
	dz = b.z - a.z;
	v1 = osg::Vec3d(dx, dy, dz);
	dx = c.x - a.x;
	dy = c.y - a.y;
	dz = c.z - a.z;
	v2 = osg::Vec3d(dx, dy, dz);
	n1.x() = (v1.y() * v2.z()) - (v1.z() * v2.y());
	n1.y() = -((v2.z() * v1.x()) - (v2.x() * v1.z()));
	n1.z() = (v1.x() * v2.y()) - (v1.y() * v2.x());
	n1.normalize();
	PXCPoint3DF32 res;
	res.x = n1.x();
	res.y = n1.y();
	res.z = n1.z();

	
	return res;
}
PXCPoint3DF32 CalculateNormal(PXCPoint3DF32 c, PXCPoint3DF32 a, PXCPoint3DF32 b, PXCPoint3DF32 d)
{
	osg::Vec3d v1;
	osg::Vec3d v2;
	osg::Vec3d n1;
	double dx, dy, dz;
	//  Vector 1 = Vertex B - Vertex A
	//	Vector 2 = Vertex C - Vertex A
	dx = b.x - d.x;
	dy = b.y - d.y;
	dz = b.z - d.z;
	v1 = osg::Vec3d(dx, dy, dz);
	dx = c.x - a.x;
	dy = c.y - a.y;
	dz = c.z - a.z;
	v2 = osg::Vec3d(dx, dy, dz);
	n1.x() = (v1.y() * v2.z()) - (v1.z() * v2.y());
	n1.y() = -((v2.z() * v1.x()) - (v2.x() * v1.z()));
	n1.z() = (v1.x() * v2.y()) - (v1.y() * v2.x());
	n1.normalize();
	PXCPoint3DF32 res;
	res.x = n1.x();
	res.y = n1.y();
	res.z = n1.z();


	return res;
}

void CalculateNormalTest()
{
	PXCPoint3DF32 c,  a,  b;
	c.x = -10;
	c.y = 10;
	c.z = -5;
	a.x = 5;
	a.y = 10.0;
	a.z = 0;
	b.x = 5;
	b.y = -10;
	b.z = 0;
	PXCPoint3DF32 n = CalculateNormal(c, a, b);
	osg::Vec3d dir(n.x, n.y,n.z);
	osg::Vec2d hp = Utilities::DirectionToHP(dir);

}
//http://fullonsoftware.co.uk/snippets/content/Math_-_Calculating_Face_Normals.pdf

 PXCPoint3DF32 starNormal(PXCPoint3DF32 p)
 {
	 //            p1
	 //     p2....p......p3
	 //           p4
	 PXCPoint3DF32 p1, p2, p3, p4,p0;
	 PXCPoint3DF32  n1, n2, n3, n4, n, n0;

	 p0 = vertices[(p.y ) * 640 + p.x];
	 p1 = vertices[(p.y - 1) * 640 + p.x];
	 p2 = vertices[(p.y ) * 640 + p.x-1];
	 p3 = vertices[(p.y ) * 640 + p.x+1]; 
	 p4 = vertices[(p.y + 1) * 640 + p.x]; 

	 n1 = CalculateNormal(p2, p3, p1,p4);

	 osg::Vec2d a1 = Utilities::DirectionToHP(osg::Vec3d(n1.x, n1.y, n1.z));
	 //n2 = CalculateNormal(p1, p0, p4);
	 //osg::Vec2d a2 = DirectionToHP(osg::Vec3d(n2.x, n2.y, n2.z));
	 //n3.x = n1.x + n2.x;
	 //n3.y = n1.y + n2.y;
	 //n3.z = n1.z + n2.z;
	 //osg::Vec2d a3 = DirectionToHP(osg::Vec3d(n1.x+n2.x, n1.y+n2.y, n1.z+n2.z) );
	 return n1;
 }
 PXCPoint3DF32 GetNormal2(PXCPointF32 point, PXCImage* rgb, PXCImage* depth)
 {
	 if (ffN == NULL)
	 {
		 ffN = fopen("normals.tx", "w");
	 }
	 PXCPoint3DF32 zero;

	 //http://fullonsoftware.co.uk/snippets/content/Math_-_Calculating_Face_Normals.pdf

	 double centerX, centerY, width, height;
	 getDepthRectangle(point, rgb, depth, centerX, centerY, width, height);
	 fprintf(ffN,"P(%lf,%lf)C(%lf,%lf,%lf,%lf)", point.x, point.y, centerX, centerY, width, height);
	 fflush(ffN);

	 PXCPointF32 normalized;
	 vector<PXCPointF32> colorPoints;
	 vector<PXCPointF32> depthPoints;
	 PXCImage::ImageInfo infoColor = rgb->QueryInfo();

	 NormalizePoint(0, 0, infoColor.width, infoColor.height, point.x, point.y, normalized);
	 colorPoints.push_back(normalized);
 	 ColorToDepth(rgb, depth, colorPoints, depthPoints);
	 PXCImage::ImageInfo infoDepth = depth->QueryInfo();
	 int IndexX, IndexY;
	 IndexX = depthPoints[0].x *infoDepth.width;
	 IndexY = depthPoints[0].y *infoDepth.height;
	 if (depthPoints[0].x == 1)
	 {
		 int fdhsk = 3;
	 }

	 //color world
	 double alfaX, betaX;
	 double alfaY, betaY;
	 betaX = (point.x - (centerX - width)) / (2 * width);
	 alfaX = 1.0 - betaX;

	 betaY = (point.y - (centerY - height)) / (2 * height);
	 alfaY = 1.0 - betaY;

	//depth world
	 int x0 = IndexX;
	 int y0 = IndexY;
	 fprintf(ffN, "Alfa(%lf,%lf)", alfaX,alfaY);
	 fflush(ffN);

	 PXCPoint3DF32 p1, p2, p3, p4, p1b, p1c, p2b, p2c, p3b, p3c, p4b, p4c;
//	 PXCPointF32 p;
	 PXCPoint3DF32  n1, n2, n3, n4, n, n0;
	 ///      p3.....p4
	 ///      .      .
	 ///      p1.....p2

	 //now check if is an internal point;
	 //if (betaX != 0 && betaY != 0)
	 //{
		 p1.x = x0;
		 p1.y = y0;

		 p2.x = x0+1;
		 p2.y = y0;

		 p3.x = x0;
		 p3.y = y0+1;

		 p4.x = x0 + 1;
		 p4.y = y0 + 1;

		 n1 = starNormal(p1);
		 osg::Vec2d a1 = Utilities::DirectionToHP(osg::Vec3d(n1.x, n1.y, n1.z));
		 n2 = starNormal(p2);
		 osg::Vec2d a2 = Utilities::DirectionToHP(osg::Vec3d(n2.x, n2.y, n2.z));
		 n3 = starNormal(p3);
		 osg::Vec2d a3 = Utilities::DirectionToHP(osg::Vec3d(n3.x, n3.y, n3.z));
		 n4 = starNormal(p4);
		 osg::Vec2d a4 = Utilities::DirectionToHP(osg::Vec3d(n4.x, n4.y, n4.z));
		 //n1 = CalculateNormal(p3, p1, p2);
		 //n2 = CalculateNormal(p1, p2, p4);
		 //n3 = CalculateNormal(p4, p3, p1);
		 //n4 = CalculateNormal(p2, p4, p3);
		 fprintf(ffN, "n1(%lf,%lf,%lf)A(%lf,%lf)", n1.x, n1.y, n1.z,a1.x(),a1.y());
		 fflush(ffN);
		 fprintf(ffN, "n2(%lf,%lf,%lf)A(%lf,%lf)", n2.x, n2.y, n2.z, a2.x(), a2.y());
		 fflush(ffN);
		 fprintf(ffN, "n3(%lf,%lf,%lf)A(%lf,%lf)", n3.x, n3.y, n3.z, a3.x(), a3.y());
		 fflush(ffN);
		 fprintf(ffN, "n4(%lf,%lf,%lf)A(%lf,%lf)", n4.x, n4.y, n4.z, a4.x(), a4.y());
		 fflush(ffN);

		 n = bilinearInterpolation(alfaX, alfaY, n1, n2, n3, n4);
		 osg::Vec2d a5 = Utilities::DirectionToHP(osg::Vec3d(n.x, n.y, n.z));
		 fprintf(ffN, "n0(%lf,%lf,%lf)A(%lf,%lf)\n", n.x, n.y, n.z, a5.x(), a5.y());
		 fflush(ffN);

	 //}
	 //else
	 //{
		// if ((betaX == 0) && (betaY != 0))
		// {
		//	 //calculate the normal on x0,y0 point
		//	 p.x = x0 - 1;
		//	 p.y = y0 - 1;
		//	 InterpolateVertex(p, p1);
		//	 p.x = x0 + 1;
		//	 p.y = y0 - 1;
		//	 InterpolateVertex(p, p2);
		//	 p.x = x0 - 1;
		//	 p.y = y0 + 1;
		//	 InterpolateVertex(p, p3);
		//	 p.x = x0 + 1;
		//	 p.y = y0 + 1;
		//	 InterpolateVertex(p, p4);
		//	 n1 = CalculateNormal(p3, p1, p2);
		//	 n2 = CalculateNormal(p1, p2, p4);
		//	 n3 = CalculateNormal(p4, p3, p1);
		//	 n4 = CalculateNormal(p2, p4, p3);
		//	 n0 = bilinearInterpolation(0.5, 0.5, n1, n2, n3, n4); //because is for x
		//	 //calculate the normal on x0,y1 point
		//	 p.x = x0 - 1;
		//	 p.y = y1 - 1;
		//	 InterpolateVertex(p, p1);
		//	 p.x = x0 + 1;
		//	 p.y = y1 - 1;
		//	 InterpolateVertex(p, p2);
		//	 p.x = x0 - 1;
		//	 p.y = y1 + 1;
		//	 InterpolateVertex(p, p3);
		//	 p.x = x0 + 1;
		//	 p.y = y1 + 1;
		//	 InterpolateVertex(p, p4);
		//	 n1 = CalculateNormal(p3, p1, p2);
		//	 n2 = CalculateNormal(p1, p2, p4);
		//	 n3 = CalculateNormal(p4, p3, p1);
		//	 n4 = CalculateNormal(p2, p4, p3);
		//	 n1 = bilinearInterpolation(0.5, 0.5, n1, n2, n3, n4); //because is for x
		//	 n = linearInterpolation(alfaY, n0, n1);
		// }
		// else
		// {
		//	 if ((betaX != 0) && (betaY == 0))
		//	 {
		//		 //calculate the normal on x0,y0 point
		//		 p.x = x0 - 1;
		//		 p.y = y0 - 1;
		//		 InterpolateVertex(p, p1);
		//		 p.x = x0 + 1;
		//		 p.y = y0 - 1;
		//		 InterpolateVertex(p, p2);
		//		 p.x = x0 - 1;
		//		 p.y = y0 + 1;
		//		 InterpolateVertex(p, p3);
		//		 p.x = x0 + 1;
		//		 p.y = y0 + 1;
		//		 InterpolateVertex(p, p4);
		//		 n1 = CalculateNormal(p3, p1, p2);
		//		 n2 = CalculateNormal(p1, p2, p4);
		//		 n3 = CalculateNormal(p4, p3, p1);
		//		 n4 = CalculateNormal(p2, p4, p3);
		//		 n0 = bilinearInterpolation(0.5, 0.5, n1, n2, n3, n4); //because is for y
		//		 //calculate the normal on x1,y0 point
		//		 p.x = x1 - 1;
		//		 p.y = y0 - 1;
		//		 InterpolateVertex(p, p1);
		//		 p.x = x1 + 1;
		//		 p.y = y0 - 1;
		//		 InterpolateVertex(p, p2);
		//		 p.x = x1 - 1;
		//		 p.y = y0 + 1;
		//		 InterpolateVertex(p, p3);
		//		 p.x = x1 + 1;
		//		 p.y = y0 + 1;
		//		 InterpolateVertex(p, p4);
		//		 n1 = CalculateNormal(p3, p1, p2);
		//		 n2 = CalculateNormal(p1, p2, p4);
		//		 n3 = CalculateNormal(p4, p3, p1);
		//		 n4 = CalculateNormal(p2, p4, p3);
		//		 n1 = bilinearInterpolation(0.5, 0.5, n1, n2, n3, n4); //because is for x
		//		 n = linearInterpolation(alfaX, n0, n1);
		//	 }
		//	 else
		//	 {
		//		 //when is fix to vertex
		//		 //calculate the normal on x0,y0 point
		//		 p.x = x0 - 1;
		//		 p.y = y0 - 1;
		//		 InterpolateVertex(p, p1);
		//		 p.x = x0 + 1;
		//		 p.y = y0 - 1;
		//		 InterpolateVertex(p, p2);
		//		 p.x = x0 - 1;
		//		 p.y = y0 + 1;
		//		 InterpolateVertex(p, p3);
		//		 p.x = x0 + 1;
		//		 p.y = y0 + 1;
		//		 InterpolateVertex(p, p4);
		//		 n1 = CalculateNormal(p3, p1, p2);
		//		 n2 = CalculateNormal(p1, p2, p4);
		//		 n3 = CalculateNormal(p4, p3, p1);
		//		 n4 = CalculateNormal(p2, p4, p3);
		//		 n = bilinearInterpolation(0.5, 0.5, n1, n2, n3, n4); //because is for y
		//	 }
		// }
	 //}
	 //PXCPoint3DF32 res;
	 //res.x = n.x();
	 //res.y = n.y();
	 //res.z = n.z();
	 return n;
 }



 bool getDepthRectangle(PXCPointF32 point, PXCImage* rgb, PXCImage* depth,double& centerX,double& centerY,double& width,double& height)
 {
	 double x0, y0;
	 x0 = floor(point.x);
	 y0 = floor(point.y);
	 PXCImage::ImageInfo infoColor = rgb->QueryInfo();
	 PXCPointF32 normalized;
	 vector<PXCPointF32> colorPoints;
	 vector<PXCPointF32> depthPoints;
	 vector<PXCPoint3DF32> world;
	 //for (int y = y0 - 3; y <= y0 + 3; y++)
	 //{
		// for (int x = x0 - 3; x <= x0 + 3; x++)
		// {
			 NormalizePoint(0, 0, infoColor.width, infoColor.height, point.x, point.y, normalized);
			 colorPoints.push_back(normalized);
		// }
	 //}
	 ColorToDepth(rgb, depth, colorPoints, depthPoints);

	 int dIndexX, dIndexY;
	 PXCImage::ImageInfo infoDepth = depth->QueryInfo();
	 for (int i = 0; i < depthPoints.size(); i++)
	 {
		 dIndexX = depthPoints[i].x *infoDepth.width;
		 dIndexY = depthPoints[i].y *infoDepth.height;
	}
	 colorPoints.clear();
	 depthPoints.clear();
	 NormalizePoint(0, 0, infoDepth.width, infoDepth.height, dIndexX , dIndexY , normalized);
	 depthPoints.push_back(normalized);
	 NormalizePoint(0, 0, infoDepth.width, infoDepth.height, dIndexX, dIndexY+1, normalized);
	 depthPoints.push_back(normalized);
	 NormalizePoint(0, 0, infoDepth.width, infoDepth.height, dIndexX+1, dIndexY, normalized);
	 depthPoints.push_back(normalized);
	 NormalizePoint(0, 0, infoDepth.width, infoDepth.height, dIndexX + 1, dIndexY + 1, normalized);
	 depthPoints.push_back(normalized);

	 DepthToColor(rgb, depth, depthPoints, colorPoints);
	 for (int i = 0; i < colorPoints.size(); i++)
	 {
		 colorPoints[i].x = colorPoints[i].x *infoColor.width;
		 colorPoints[i].y = colorPoints[i].y *infoColor.height;
	 }
	 
	 if (colorPoints[0].x < 0)
	 {
		 int gkjedgfuk = 3;
		 return false;
	 }

	 centerX = colorPoints[0].x;
	 centerY = colorPoints[0].y;
	 width = (colorPoints[2].x - colorPoints[0].x)/2.0;
	 height = (colorPoints[1].y - colorPoints[0].y) / 2.0;
	 return true;
 }
 PXCPoint3DF32 ColorToWorld(PXCPointF32 point, PXCImage* rgb, PXCImage* depth, PXCPoint3DF32& normal)
{
	PXCPointF32 normalized;
	vector<PXCPoint3DF32> world;
	vector<PXCPoint3DF32> normals;
	vector<PXCPointF32> colorPoints;
	vector<PXCPointF32> depthPoints;
	PXCImage::ImageInfo infoColor = rgb->QueryInfo();
	//world.x	 = -1000;
	//world.y = -1000;
	//world.z = -1000;
	double centerX, centerY, width, height;
	if(! getDepthRectangle(point, rgb, depth, centerX, centerY, width, height))
	{
		normal.x = 0;
		normal.y = 0;
		normal.z = 0;
		return normal;
	}
		
	double alfaX, betaX, alfaY, betaY;
	double x0,y0;
	x0 = floor(point.x);
	betaX = point.x - x0;
	alfaX = 1.0 - betaX;
	y0 = floor(point.y);
	betaY = point.y - y0;
	alfaY = 1.0 - betaY;

	NormalizePoint(0, 0, infoColor.width, infoColor.height, point.x, point.y, normalized);
	colorPoints.push_back(normalized);

	
		ColorToDepth(rgb, depth, colorPoints, depthPoints);
		//drawDepthImage(depth, depthPoints[0]);
		
		PXCImage::ImageInfo infoDepth = depth->QueryInfo();
		for(int i = 0; i < depthPoints.size();i++  )
		{
			depthPoints[i].x *= infoDepth.width;
			depthPoints[i].y *= infoDepth.height;
			if ((0 <= depthPoints[i].x && depthPoints[i].x < 640) && (0 <= depthPoints[i].y && depthPoints[i].y < 480))
			{
				//InterpolateVertex(depthPoints[0], world);
				PXCPoint3DF32 temp_world = vertices[depthPoints[i].y * 640 + depthPoints[i].x];
				world.push_back(temp_world);
//				normals.push_back(GetNormal2(depthPoints[i]));
			}
			else
			{
				int x = 5;
			}
		}
		normal = GetNormal2(point, rgb, depth);
		PXCPoint3DF32 result;
		result.x = result.y = result.z = 0;
		//if (world.size() == 4)
		//{
		//	result =  bilinearInterpolation(alfaX, alfaY, world[0], world[1], world[2], world[3]);
		//	if ((result.x != 0) && (result.y != 0) && (result.z != 0))
		//	{
		//		normal = bilinearInterpolation(alfaX, alfaY, normals[0], normals[1], normals[2], normals[3]);
		//	}
		//	else
		//	{
		//		normal.x = normal.y = normal.z = 0.0;
		//	}
		//}
	//}
		return result;
}
//void processLandMarks(PXCFaceData::Face* trackedFace, PXCImage* rgb, PXCFaceData::HeadPosition position, PXCImage* depth)
//{
//	const PXCFaceData::LandmarksData* landmarkData = trackedFace->QueryLandmarks();
//	PXCFaceData::LandmarkPoint centerRight, rightRight, leftRight;
//	int i = landmarkData->QueryPointIndex(PXCFaceData::LANDMARK_EYE_RIGHT_CENTER);
//	if (i > 0)
//	{
//		landmarkData->QueryPoint(i, &centerRight);
//		i = landmarkData->QueryPointIndex(PXCFaceData::LANDMARK_EYELID_RIGHT_RIGHT);
//		landmarkData->QueryPoint(i, &rightRight);
//		i = landmarkData->QueryPointIndex(PXCFaceData::LANDMARK_EYELID_RIGHT_LEFT);
//		landmarkData->QueryPoint(i, &leftRight);
//
//		//drawColorImage(rgb, centerRight.image);
//		//printf("Right Pupil(%f,%f,%f)\n", 1000.0*centerRight.world.x, 1000.0*centerRight.world.y, 1000.0*centerRight.world.z);
//		PXCPoint3DF32 normal;
//		PXCPoint3DF32 world = ColorToWorld(centerRight.image, rgb, depth,normal);
//		printf("Ivertex(%f,%f,%f)\n", world.x, world.y, world.z);
//
// 
//	}
//	
//}
osg::Vec3d N2A(osg::Vec3d n)
{
	osg::Vec3d result;
	osg::Vec3d up(0, 1, 0),w0,u0;
	result.x() = (90.0+osg::RadiansToDegrees(atan2(n.z(), n.x())));
	result.y() = (90.0 + osg::RadiansToDegrees(atan2(n.z(), n.y())));

	return result;
}
double ToRadians(double val)
{
	return acos(0)*val / 90.0;
}
osg::Vec3d pupilPos, pupilCenter, eyeCenterPos;
double diameter;	

/* calculate intersection with 3d world*/
osg::Vec3 computeTerrainIntersection(osg::Node* subgraph, osg::Vec3d x, osg::Vec3d  y)
{
	const osg::BoundingSphere& bs = subgraph->getBound();
	float zMax = bs.center().z() + bs.radius();
	float zMin = bs.center().z() - bs.radius();

	osg::ref_ptr<osgUtil::LineSegmentIntersector> intersector =
		new osgUtil::LineSegmentIntersector(x, y);

	osgUtil::IntersectionVisitor iv(intersector.get());

	subgraph->accept(iv);

	if (intersector->containsIntersections())
	{
		return intersector->getFirstIntersection().getWorldIntersectPoint();
	}

	return osg::Vec3(0, 0, 0.0f);
}
/*************************************************************
This function Get the angle between two points (h,p in degrees)

Input:
osg::Vec3d cameraPoint		: p0
osg::Vec3d tgtPoint			: p1

Output			:
osg::Vec3d					: h,p,0 (degrees)

**************************************************************/
osg::Vec3d getAngleBetween(osg::Vec3d cameraPoint, osg::Vec3d tgtPoint)
{
	osg::Vec3 cameraAngle;
	tgtPoint = tgtPoint - cameraPoint;

	cameraAngle.x() = -(90 + osg::RadiansToDegrees(atan2(tgtPoint.z(), tgtPoint.x())));
	cameraAngle.y() = 90 + osg::RadiansToDegrees(atan2(tgtPoint.z(), tgtPoint.y()));
	cameraAngle.z() = 0;
	//cameraAngle.x() *= -1.0;
	return cameraAngle;
}
osg::Vec3d getAngleBetween(osg::Vec3d cameraPoint, osg::Vec3d headPosition, osg::Vec3d headAngle, osg::Vec3d tgtPoint)
{

	osg::Matrixd invHead = Utilities::worldToHead(headPosition, headAngle);
	tgtPoint = tgtPoint * 	invHead;
	cameraPoint = cameraPoint *	invHead;
	osg::Vec3 cameraAngle = getAngleBetween(cameraPoint, tgtPoint);

	fprintf(fFile, "tgtPoint(%lf,%lf,%lf) ,cameraPoint(%lf,%lf,%lf),cameraAngle(%lf,%lf,%lf)	\n",
		tgtPoint.x(), tgtPoint.y(), tgtPoint.z(),
		cameraPoint.x(), cameraPoint.y(), cameraPoint.z(),
		cameraAngle.x(), cameraAngle.y(), cameraAngle.z());
	fflush(fFile);
	return cameraAngle;


}


bool getLandmarkPixel(const PXCFaceData::LandmarksData* landmarkData, PXCFaceData::LandmarkType land, PXCCapture::Sample *sample, osg::Vec2d& pixel)
{
	PXCFaceData::LandmarkPoint point;
	PXCPoint3DF32 n;
	int i = landmarkData->QueryPointIndex(land);
	if (i > 0)
	{
		landmarkData->QueryPoint(i, &point);
		if ((point.confidenceWorld != 100) || (point.confidenceImage != 100))
		{
			return false;
		}
	}
	else
		return false;
	pixel.x() = point.image.x;
	pixel.y() = point.image.y;

}
bool getLandMarkOnWorld(const PXCFaceData::LandmarksData* landmarkData, PXCFaceData::LandmarkType land, PXCCapture::Sample *sample, osg::Vec3d& normal, osg::Vec3d& world)
{
	PXCPoint3DF32 temp;
	PXCFaceData::LandmarkPoint point;
	PXCPoint3DF32 n;
	int i = landmarkData->QueryPointIndex(land);
	if (i > 0)
	{
		landmarkData->QueryPoint(i, &point);
		if ((point.confidenceWorld != 100)||(point.confidenceImage != 100))
		{
			return false;
		}

		temp = ColorToWorld(point.image, sample->color, sample->depth,n);
		world = osg::Vec3d(point.world.x, point.world.y, point.world.z)*1000.0;
		normal = osg::Vec3d(n.x, n.y,n.z);
		//drawColorImage(sample->color, point.image);
 		//SaveDepthWindow(&point, sample, 2);
		if ((normal.x() == 0) && (normal.y() == 0) && (normal.z() == 0))
			return false;
		
	}
	return true;
}
//void SaveDepthWindow(PXCFaceData::LandmarkPoint* point, PXCCapture::Sample *sample, int width)
//{
//	PXCPoint3DF32 temp;
//	PXCPoint3DF32 n;
//	osg::Vec3d normal;
//	osg::Vec3d world;
//	FILE* ff = fopen("DepthWindow.txt", "w");
//	int x0 = floor(point->image.x);
//	int y0 = floor(point->image.y);
//	for (int y = y0 - 3*(width / 2); y < y0 + 3*(width / 2)+1; y+=3)
//	{
//		for (int x = x0 - 3 * (width / 2); x < x0 + 3 * (width / 2)+1; x += 3)
//		{
//			temp = ColorToWorld(*point, sample->color, sample->depth, n);
//			temp = point->world;
//			world = osg::Vec3d(temp.x, temp.y, temp.z);
//			normal = osg::Vec3d(n.x, n.y, n.z);
//			fprintf(ff, "[%d,%d]", x, y);
//			fprintf(ff, "W(%lf,%lf,%lf)", temp.x, temp.y, temp.z);
//			fprintf(ff, "N(%lf,%lf,%lf)\n", n.x, n.y, n.z);
//			fflush(ff);
//		}
//	}
//	temp = ColorToWorld(point->image.x, point->image.y, sample->color, sample->depth, n);
//	temp = point->world;
//	world = osg::Vec3d(temp.x, temp.y, temp.z);
//	normal = osg::Vec3d(n.x, n.y, n.z);
//	fprintf(ff, "[%lf,%lf]", point->image.x, point->image.y);
//	fprintf(ff, "W(%lf,%lf,%lf)", temp.x, temp.y, temp.z);
//	fprintf(ff, "N(%lf,%lf,%lf)\n", n.x, n.y, n.z);
//	fflush(ff);
//
//	fclose(ff);
//}
osg::Matrixd getRotmatrix(osg::Vec3d angle)
{
	osg::Matrixd rMatrix;
	rMatrix.makeRotate(
		osg::DegreesToRadians(angle.x()), osg::Vec3(0, 1, 0),//heading
		osg::DegreesToRadians(angle.y()), osg::Vec3(1, 0, 0),//pitch
		osg::DegreesToRadians(angle.z()), osg::Vec3(0, 0, 1));//roll	- 
		
	return rMatrix;
}

osg::Vec3d getRelativeAngle(osg::Vec3d tgt, osg::Vec3d camera, osg::Vec3d headAngle)
{
	osg::Matrixd headRotate = getRotmatrix(headAngle);
	osg::Matrixd  headRotateInv;
	headRotateInv.invert(headRotate);
	
	osg::Vec3d finalAngle = getAngleBetween(camera, tgt);
	osg::Matrixd finalAngleMat = getRotmatrix(finalAngle);

	osg::Matrixd RelativeAngle = finalAngleMat * headRotateInv;
	return 	   Utilities::MatrixGetRotation(RelativeAngle);
	//double rrx, rry, rrz;
	//MatrixGetRotation(RelativeAngle, rrz, rrx, rry);
	//return osg::Vec3d(-rry, rrx, rrz);
}
void SaveLiDD(const PXCFaceData::LandmarksData* landmarkData, PXCCapture::Sample *sample, osg::Vec3d position, osg::Vec3d angles)
{
	if (fLiddown == NULL)
	{
		fLiddown = fopen("LidDown.txt", "w");
	}
	osg::Vec3d eyeLid2_BW, eyeLid_BW, eyeLid2_BH, eyeLid_BH;
	osg::Vec3d normal;
	bool resL = getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_LEFT_BOTTOM, sample, normal, eyeLid_BW);
	bool resR = getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_RIGHT_BOTTOM, sample, normal, eyeLid2_BW);
	if (resL == false || resR == false)
		return;
	osg::Matrixd   _worldTohead, rMatrix, _headToWorld;
	_worldTohead = Utilities::worldToHead(position, angles);
	_headToWorld = Utilities::headToWorld(position, angles);


	osg::Matrixd headPosition;
	headPosition.makeTranslate(position.x(), position.y(), position.z());
	osg::Matrixd headPositionI;
	headPositionI.invert(headPosition);
	osg::Matrixd aa = getRotmatrix(angles);
	osg::Matrixd aai;
	aai.invert(aa);


	eyeLid2_BH = eyeLid2_BW* _worldTohead;
	eyeLid_BH = eyeLid_BW* _worldTohead;
	fprintf(fLiddown, "%lf %lf %lf %lf %lf %lf ", position.x(), position.y(), position.z(), angles.x(), angles.y(), angles.z());
	fprintf(fLiddown, "%lf %lf %lf %lf %lf %lf ", eyeLid_BW.x(), eyeLid_BW.y(), eyeLid_BW.z(), eyeLid2_BW.x(), eyeLid2_BW.y(), eyeLid2_BW.z());

	fprintf(fLiddown, "%lf %lf %lf %lf %lf %lf\n", eyeLid_BH.x(), eyeLid_BH.y(), eyeLid_BH.z(), eyeLid2_BH.x(), eyeLid2_BH.y(), eyeLid2_BH.z());
	fflush(fLiddown);


}
void SaveEyes()
{
	osg::Vec3d avg;

	FILE* ff = fopen("EYE_LEFT_CENTER.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in EYE_LEFT_CENTER_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= EYE_LEFT_CENTER_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
	 ff = fopen("EYELID_LEFT_LEFT.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in EYELID_LEFT_LEFT_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= EYELID_LEFT_LEFT_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
	 ff = fopen("EYELID_LEFT_RIGHT.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in EYELID_LEFT_RIGHT_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= EYELID_LEFT_RIGHT_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
	 ff = fopen("EYELID_LEFT_TOP.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in EYELID_LEFT_TOP_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= EYELID_LEFT_TOP_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
	 ff = fopen("EYELID_LEFT_BOTTOM.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in EYELID_LEFT_BOTTOM_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= EYELID_LEFT_BOTTOM_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------

	 ff = fopen("EYE_RIGHT_CENTER.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in EYE_RIGHT_CENTER_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= EYE_RIGHT_CENTER_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
	 ff = fopen("EYELID_RIGHT_LEFT.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in EYELID_RIGHT_LEFT_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= EYELID_RIGHT_LEFT_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
	 ff = fopen("EYELID_RIGHT_RIGHT.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in EYELID_RIGHT_RIGHT_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= EYELID_RIGHT_RIGHT_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
	 ff = fopen("EYELID_RIGHT_TOP.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in EYELID_RIGHT_TOP_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= EYELID_RIGHT_TOP_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
	 ff = fopen("EYELID_RIGHT_BOTTOM.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in EYELID_RIGHT_BOTTOM_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= EYELID_RIGHT_BOTTOM_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------

	ff = fopen("HEYE_LEFT_CENTER.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in HEYE_LEFT_CENTER_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= HEYE_LEFT_CENTER_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
	ff = fopen("HEYELID_LEFT_LEFT.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in HEYELID_LEFT_LEFT_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= HEYELID_LEFT_LEFT_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
	ff = fopen("HEYELID_LEFT_RIGHT.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in HEYELID_LEFT_RIGHT_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= HEYELID_LEFT_RIGHT_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
	ff = fopen("HEYELID_LEFT_TOP.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in HEYELID_LEFT_TOP_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= HEYELID_LEFT_TOP_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
	ff = fopen("HEYELID_LEFT_BOTTOM.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in HEYELID_LEFT_BOTTOM_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= HEYELID_LEFT_BOTTOM_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------

	ff = fopen("HEYE_RIGHT_CENTER.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in HEYE_RIGHT_CENTER_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= HEYE_RIGHT_CENTER_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
	ff = fopen("HEYELID_RIGHT_LEFT.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in HEYELID_RIGHT_LEFT_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= HEYELID_RIGHT_LEFT_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
	ff = fopen("HEYELID_RIGHT_RIGHT.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in HEYELID_RIGHT_RIGHT_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= HEYELID_RIGHT_RIGHT_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
	ff = fopen("HEYELID_RIGHT_TOP.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in HEYELID_RIGHT_TOP_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= HEYELID_RIGHT_TOP_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
	ff = fopen("HEYELID_RIGHT_BOTTOM.txt", "w");
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in HEYELID_RIGHT_BOTTOM_MAP)
	{
		fprintf(ff, "%lf %lf %lf\n", val.second.x(), val.second.y(), val.second.z());
		avg += val.second;
	}
	avg /= HEYELID_RIGHT_BOTTOM_MAP.size();
	fprintf(ff, "%lf %lf %lf\n", avg.x(), avg.y(), avg.z());
	fflush(ff);
	fclose(ff);
	//--------------------------------------------------------------------------------
}
void build3DModel(const PXCFaceData::LandmarksData* landmarkData, PXCCapture::Sample *sample, osg::Vec3d position, osg::Vec3d angles)
{
	osg::Vec3d temp;
	osg::Vec3d eyeLid_L, eyeLid_R, eyeLid_T, eyeLid_B;
	osg::Vec3d eyeLid2_L, eyeLid2_R, eyeLid2_T, eyeLid2_B;

	osg::Vec3d pupilWPos, pupilCenter, pupilCenter2, pupilWPosR;
	osg::Matrixd   _worldTohead, rMatrix, _headToWorld;
	_worldTohead = Utilities::worldToHead(position, angles);
	_headToWorld = Utilities::headToWorld(position, angles);

	if (headPosFile == NULL)
	{
		headPosFile = fopen("headPosFile.txt", "w");
	}
	fprintf(headPosFile, "%lf %lf %lf %lf %lf %lf\n", position.x(), position.y(), position.z(), angles.x(), angles.y(), angles.z());

	osg::Vec3d h_eyeLid_L, h_eyeLid_R, h_eyeLidR_L, h_eyeLidR_R;
	osg::Vec3d n_pl, n_pr, eyeLid_LN, eyeLid2_RN, eyeLid_RN, eyeLid2_LN, eyeLid_TN, eyeLid_BN;
	osg::Vec3d	h_eyeLid_LN, h_eyeLid_RN, h_eyeLidR_LN, h_eyeLidR_RN, eyeLid2_TN, eyeLid2_BN;

	{
		if (getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_LEFT_CENTER, sample, n_pl, temp))
		{
			EYE_LEFT_CENTER_MAP[EYE_LEFT_CENTER_MAP.size()] = temp;
			HEYE_LEFT_CENTER_MAP[HEYE_LEFT_CENTER_MAP.size()] = temp*_worldTohead;
		}

		if (getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_LEFT_LEFT, sample, eyeLid_LN, temp))
		{
			EYELID_LEFT_LEFT_MAP[EYELID_LEFT_LEFT_MAP.size()] = temp;
			HEYELID_LEFT_LEFT_MAP[HEYELID_LEFT_LEFT_MAP.size()] = temp*_worldTohead;
		}

		if (getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_LEFT_RIGHT, sample, eyeLid_RN, temp))
		{
			EYELID_LEFT_RIGHT_MAP[EYELID_LEFT_RIGHT_MAP.size()] = temp;
			HEYELID_LEFT_RIGHT_MAP[HEYELID_LEFT_RIGHT_MAP.size()] = temp*_worldTohead;
		}

		if (getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_LEFT_TOP, sample, eyeLid_TN, temp))
		{
			EYELID_LEFT_TOP_MAP[EYELID_LEFT_TOP_MAP.size()] = temp;
			HEYELID_LEFT_TOP_MAP[HEYELID_LEFT_TOP_MAP.size()] = temp*_worldTohead;
		}

		if (getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_LEFT_BOTTOM, sample, eyeLid_BN, temp))
		{
			EYELID_LEFT_BOTTOM_MAP[EYELID_LEFT_BOTTOM_MAP.size()] = temp;
			HEYELID_LEFT_BOTTOM_MAP[HEYELID_LEFT_BOTTOM_MAP.size()] = temp*_worldTohead;
		}
	}

	{
		if (getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_RIGHT_CENTER, sample, n_pr, temp))
		{
			EYE_RIGHT_CENTER_MAP[EYE_RIGHT_CENTER_MAP.size()] = temp;
			HEYE_RIGHT_CENTER_MAP[HEYE_RIGHT_CENTER_MAP.size()] = temp*_worldTohead;
		}

		if (getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_RIGHT_LEFT, sample, eyeLid2_LN, temp))
		{
			EYELID_RIGHT_LEFT_MAP[EYELID_RIGHT_LEFT_MAP.size()] = temp;
			HEYELID_RIGHT_LEFT_MAP[HEYELID_RIGHT_LEFT_MAP.size()] = temp*_worldTohead;
		}

		if (getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_RIGHT_RIGHT, sample, eyeLid2_RN, temp))
		{
			EYELID_RIGHT_RIGHT_MAP[EYELID_RIGHT_RIGHT_MAP.size()] = temp;
			HEYELID_RIGHT_RIGHT_MAP[HEYELID_RIGHT_RIGHT_MAP.size()] = temp*_worldTohead;
		}

		if (getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_RIGHT_TOP, sample, eyeLid2_TN, temp))
		{
			EYELID_RIGHT_TOP_MAP[EYELID_RIGHT_TOP_MAP.size()] = temp;
			HEYELID_RIGHT_TOP_MAP[HEYELID_RIGHT_TOP_MAP.size()] = temp*_worldTohead;
		}

		if (getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_RIGHT_BOTTOM, sample, eyeLid2_BN, temp))
		{
			EYELID_RIGHT_BOTTOM_MAP[EYELID_RIGHT_BOTTOM_MAP.size()] = temp;
			HEYELID_RIGHT_BOTTOM_MAP[HEYELID_RIGHT_BOTTOM_MAP.size()] = temp*_worldTohead;
		}

	}
	//------------------------------------
	osg::Vec3d avg;
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in HEYELID_LEFT_BOTTOM_MAP)
	{
		avg += val.second;
	}
	leftEyeCenter = avg /  HEYELID_LEFT_BOTTOM_MAP.size();
	//...........................................
	 
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in HEYE_LEFT_CENTER_MAP)
	{
		avg += val.second;
	}
	avg = avg / HEYE_LEFT_CENTER_MAP.size();
	leftEyeCenter.z() = avg.z();
	//------------------------------------
	//------------------------------------
	 
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in HEYELID_RIGHT_BOTTOM_MAP)
	{
		avg += val.second;
	}
	rightEyeCenter = avg / HEYELID_RIGHT_BOTTOM_MAP.size();
	//...........................................
	 
	avg = osg::Vec3d(0, 0, 0);
	for each (auto val in HEYE_RIGHT_CENTER_MAP)
	{
		avg += val.second;
	}
	avg = avg / HEYE_RIGHT_CENTER_MAP.size();
	rightEyeCenter.z() = avg.z();
	//------------------------------------
}
bool  GetEyeCenter(const PXCFaceData::LandmarksData* landmarkData, PXCCapture::Sample *sample, osg::Vec3d& left ,osg::Vec3d& right)
{
	osg::Vec3d eyeLid_L, eyeLid_R, eyeLid_T, eyeLid_B;
	osg::Vec3d eyeLid2_L, eyeLid2_R, eyeLid2_T, eyeLid2_B;
	osg::Vec3d pupilWPos, pupilCenter, pupilCenter2, pupilWPosR;

	osg::Vec3d eyeLid_LN, eyeLid_RN, eyeLid_TN, eyeLid_BN;
	osg::Vec3d eyeLid2_LN, eyeLid2_RN, eyeLid2_TN, eyeLid2_BN;
	osg::Vec3d pupilWPosN, pupilCenterN, pupilCenter2N, pupilWPosRN;

	{
		//pupilWPos = getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_LEFT_CENTER, sample, pupilWPosN);
		if (!getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_LEFT_TOP, sample, eyeLid_TN, eyeLid_T) ||
			!getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_LEFT_BOTTOM, sample, eyeLid_BN, eyeLid_B))
			return false;
		//pupilCenter = osg::Vec3d((5.5 / 11.0)*eyeLid_L.x() + (5.5 / 11.0)*eyeLid_R.x(), eyeLid_B.y() , (eyeLid_T.z() + eyeLid_B.z()) / 2.0);
		pupilCenter = osg::Vec3d(eyeLid_B.x(), eyeLid_B.y(), (eyeLid_T.z() + eyeLid_B.z()) / 2.0-2.0);
		left = pupilCenter;
		if ((eyeLid_T.x() == 0 && eyeLid_T.y() == 0 && eyeLid_T.z() == 0) ||
			(eyeLid_B.x() == 0 && eyeLid_B.y() == 0 && eyeLid_B.z() == 0))
			return false;

	}

	{
		if (!getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_RIGHT_TOP, sample, eyeLid2_TN, eyeLid2_T) ||
			!getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_RIGHT_BOTTOM, sample, eyeLid2_BN, eyeLid2_B))
			return false;

		//pupilCenter2 = osg::Vec3d((5.5 / 11.0)*eyeLid2_L.x() + (5.5 / 11.0)*eyeLid2_R.x(), eyeLid2_B.y() , (eyeLid2_T.z() + eyeLid2_B.z()) / 2.0);
		pupilCenter2 = osg::Vec3d(eyeLid2_B.x(), eyeLid2_B.y(), (eyeLid2_T.z() + eyeLid2_B.z()) / 2.0-2.0);
		right = pupilCenter2;
		if ((eyeLid2_T.x() == 0 && eyeLid2_T.y() == 0 && eyeLid2_T.z() == 0) ||
			(eyeLid2_B.x() == 0 && eyeLid2_B.y() == 0 && eyeLid2_B.z() == 0))
			return false;

	}
	return true;
}
//bool AnotherCalibrationNormals(cv::Mat& colorImage, const PXCFaceData::LandmarksData* landmarkData,/* int counter,*/ PXCCapture::Sample *sample,  /*osg::Vec3d eyeCenter,*/
//	osg::Vec3d position, osg::Vec3d angles, osg::Vec3d _leftEyeCenter, osg::Vec3d _rightEyeCenter, osg::Vec3d tgtWPos, cv::Point tgtImagePos, std::string title/*, osg::Vec3d& idealAim, osg::Vec3d& Aim*/)
//{
//	double Zeye = 11.0;
//	cv::circle(colorImage, tgtImagePos, 5, cv::Scalar(0, 0, 255), -1);
//	//take pupil
//	osg::Vec3d pupilWPosR, pupilWPosL;
//	osg::Vec3d pupilWPosRN, pupilWPosLN;
//	bool resL= getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_LEFT_CENTER, sample, pupilWPosLN, pupilWPosL);
//	bool resR= getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_RIGHT_CENTER, sample, pupilWPosRN, pupilWPosR);
//	if (!resL || !resR)
//		return false;
//	//--------------------------------------------------------------------------------------------
//	if ((pupilWPosL.x() == pupilWPosL.y() && pupilWPosL.x() == pupilWPosL.z() && pupilWPosL.x() == 0) ||
//		(pupilWPosR.x() == pupilWPosR.y() && pupilWPosR.x() == pupilWPosR.z() && pupilWPosR.x() == 0))
//		return false;
//
//	osg::Vec2d leftHP = Utilities::DirectionToHP(pupilWPosLN);
//	osg::Vec2d rightHP = Utilities::DirectionToHP(pupilWPosRN);
//
//	//-----------------------------------------------------------------------------------------------
//	//Calculate Alfa to 0,0
//	osg::Vec3d 		   tgtW(0, 0, 0);
//	osg::Vec3d angleL, angleR, naL, naR;
//	angleL = getAngleBetween(pupilWPosL, tgtWPos);
//	angleR = getAngleBetween(pupilWPosR, tgtWPos);
//	osg::Matrixd lm =getRotmatrix(osg::Vec3d(leftHP.x(), leftHP.y(), 0));
//	osg::Matrixd rm = getRotmatrix(osg::Vec3d(rightHP.x(), rightHP.y(), 0));
//	osg::Matrixd lmI, rmI;
//	lmI.invert(lm);
//	rmI.invert(rm);
//	osg::Matrixd angleLM, angleRM;
//	osg::Matrixd angleLM2 =angleLM = getRotmatrix(angleL);
//	osg::Matrixd angleRM2 = angleRM = getRotmatrix(angleR);
//
//	angleLM2 = lmI *  angleLM;
//	angleRM2 = rmI *  angleRM;
//	
//	osg::Vec3d   delta_L = Utilities::MatrixGetRotation(angleLM2);
//	osg::Vec3d   delta_R = Utilities::MatrixGetRotation(angleRM2);
//	  //second opinion
//	osg::Vec3d   delta_L2 = Utilities::MatrixGetRotation(angleLM*lmI);
//	osg::Vec3d   delta_R2 = Utilities::MatrixGetRotation(angleRM*rmI);
//
//
//	{
//		LeftCenterFixMap[LeftCenterDeltaMap.size()] = delta_L;
//		LCenterDeltaMap[LCenterDeltaMap.size()] = delta_L;
//	}
//	//	else
//	int jouhvibk = 67;
//	//	if (delta_R.z() > 0 && delta_R.y() > 0)
//	{
//		RightCenterDeltaMap[RightCenterDeltaMap.size()] = delta_R;
//		RCenterDeltaMap[RCenterDeltaMap.size()] = delta_R;
//	}
//	//else
//
//
//	//--------------------------------------------------------------------------------------------------
//	//reconstruct
//	//
//	//-------------------------------------------------------------------------------------------------
//	//intersection
//	return true;
//
//}

//bool AnotherCalibrationFixedZ(cv::Mat& colorImage, const PXCFaceData::LandmarksData* landmarkData,/* int counter,*/ PXCCapture::Sample *sample,  /*osg::Vec3d eyeCenter,*/
//	osg::Vec3d position, osg::Vec3d angles, osg::Vec3d _leftEyeCenter, osg::Vec3d _rightEyeCenter, osg::Vec3d tgtWPos, cv::Point tgtImagePos, std::string title/*, osg::Vec3d& idealAim, osg::Vec3d& Aim*/)
//{
//	double Zeye = 11.0;
//	cv::circle(colorImage, tgtImagePos, 5, cv::Scalar(0, 0, 255), -1);
//	//take pupil
//	osg::Vec3d pupilWPosR, pupilWPosL;
//	osg::Vec3d pupilWPosRN, pupilWPosLN;
//	bool resL = getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_LEFT_CENTER, sample, pupilWPosLN, pupilWPosL);
//	bool resR = getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_RIGHT_CENTER, sample, pupilWPosRN, pupilWPosR);
//	if (!resL || !resR)
//		return false;
//	//--------------------------------------------------------------------------------------------
//	if ((pupilWPosL.x() == pupilWPosL.y() && pupilWPosL.x() == pupilWPosL.z() && pupilWPosL.x() == 0) ||
//		(pupilWPosR.x() == pupilWPosR.y() && pupilWPosR.x() == pupilWPosR.z() && pupilWPosR.x() == 0))
//		return false;
//
//	osg::Matrixd   _worldTohead, rMatrix, _headToWorld;
//	_worldTohead = Utilities::worldToHead(position, angles);
//	_headToWorld = Utilities::headToWorld(position, angles);
//	osg::Vec3d	pupilWPosRH, pupilWPosLH;
//
//	pupilWPosLH = pupilWPosL  * _worldTohead;
//	pupilWPosRH = pupilWPosR  * _worldTohead;
//
//	//-----------------------------------------------------------------------------------------------
//	//Calculate Alfa to 0,0
//	osg::Vec3d 		   tgtW(0, 0, 0);
//	osg::Vec3d angleL, angleR, naL, naR;
//	angleL = getAngleBetween(pupilWPosL, tgtWPos);
//	angleR = getAngleBetween(pupilWPosR, tgtWPos);
//	naL = N2A(pupilWPosLN);
//	naR = N2A(pupilWPosRN);
//	double t = -pupilWPosL.z() / pupilWPosLN.z();
//	double wx = -(t* pupilWPosLN.x()) + pupilWPosL.x();
//	double wy = t* pupilWPosLN.y() + pupilWPosL.y();
//
//
//
//	osg::Matrixd leftM = getRotmatrix(angleL);
//	osg::Matrixd rightM = getRotmatrix(angleR);
//	osg::Matrixd headR = getRotmatrix(angles);
//	osg::Matrixd headRI;
//	headRI.invert(headR);
//
//	osg::Matrix eyeLeft = headRI*leftM;
//	osg::Matrix eyeRigth = headRI*rightM;
//
//	osg::Vec3d eyeLeftDeg = Utilities::MatrixGetRotation(eyeLeft);
//	osg::Vec3d eyeRightDeg = Utilities::MatrixGetRotation(eyeRigth);
//
//	osg::Vec3d eyeLeftDeg2 = Utilities::MatrixGetRotation(leftM*headRI);
//	osg::Vec3d eyeRightDeg2 = Utilities::MatrixGetRotation(rightM*headRI);
//
//	//-------------------------------------------------------------------------------------------------
//	//calculate center
//	osg::Vec3d cl, cr, hcl, hcr;
//
//	//refine
//	hcl = _leftEyeCenter;
//	hcr = _rightEyeCenter;
//
//	//hcl.z() += 8.0;
//	//hcr.z() += 8.0;
//	//---x-----
//	osg::Vec3d delta_L = pupilWPosLH - hcl;
//	//delta_L.z() = Zeye;
//
//	//delta_L.z() = delta_L.x() / tan(osg::DegreesToRadians(eyeLeftDeg.x())); //x/z = tan alfa   //angleL
//	//delta_L.y() += delta_L.z()*tan(osg::DegreesToRadians(-eyeLeftDeg.y()));
//	double tlx = delta_L.z() * tan(osg::DegreesToRadians(eyeLeftDeg.x())); //x/z = tan alfa   //angleL
//	double tly = delta_L.z()*tan(osg::DegreesToRadians(eyeLeftDeg.y()));
//	delta_L.x() = delta_L.x() - tlx;
//	delta_L.y() = delta_L.y() - tly;
//
//	osg::Vec3d delta_R = pupilWPosRH - hcr;
//
//	delta_R.x() -= delta_R.z() * tan(osg::DegreesToRadians(eyeRightDeg.x())); //x/z = tan alfa
//	delta_R.y() -= delta_R.z() *tan(osg::DegreesToRadians(eyeRightDeg.y()));
//
//	if (abs(delta_R.x()) > 100 || abs(delta_L.x()) > 100)
//		int jouhvibk = 67;
//
////	if (delta_L.z() > 0 && delta_L.y() > 0)
//	{
//		LeftCenterFixMap[LeftCenterFixMap.size()] = delta_L;
//		LCenterFixMap[LCenterFixMap.size()] = delta_L;
//	}
////	else
//		int jouhvibk = 67;
////	if (delta_R.z() > 0 && delta_R.y() > 0)
//	{
//		RightCenterFixMap[RightCenterFixMap.size()] = delta_R;
//		RCenterFixMap[RCenterFixMap.size()] = delta_R;
//	}
////else
//
//
//	//--------------------------------------------------------------------------------------------------
//	//reconstruct
//	//
//	//-------------------------------------------------------------------------------------------------
//	//intersection
//	return true;
//
//}

//bool AnotherCalibrationFixedX(cv::Mat& colorImage, const PXCFaceData::LandmarksData* landmarkData,/* int counter,*/ PXCCapture::Sample *sample,  /*osg::Vec3d eyeCenter,*/
//	osg::Vec3d position, osg::Vec3d angles, osg::Vec3d _leftEyeCenter, osg::Vec3d _rightEyeCenter, osg::Vec3d tgtWPos, cv::Point tgtImagePos, std::string title/*, osg::Vec3d& idealAim, osg::Vec3d& Aim*/)
//{
//
//	cv::circle(colorImage, tgtImagePos, 5, cv::Scalar(0, 0, 255), -1);
//	//take pupil
//	osg::Vec3d pupilWPosR, pupilWPosL;
//	osg::Vec3d pupilWPosRN, pupilWPosLN;
//	bool resL = getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_LEFT_CENTER, sample, pupilWPosLN, pupilWPosL);
//	bool resR = getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_RIGHT_CENTER, sample, pupilWPosRN, pupilWPosR);
//	if (!resL || !resR)
//		return false;
//	//--------------------------------------------------------------------------------------------
//	if ((pupilWPosL.x() == pupilWPosL.y() && pupilWPosL.x() == pupilWPosL.z() && pupilWPosL.x() == 0) ||
//		(pupilWPosR.x() == pupilWPosR.y() && pupilWPosR.x() == pupilWPosR.z() && pupilWPosR.x() == 0))
//		return false;
//
//	osg::Matrixd   _worldTohead, rMatrix, _headToWorld;
//	_worldTohead = Utilities::worldToHead(position, angles);
//	_headToWorld = Utilities::headToWorld(position, angles);
//	osg::Vec3d	pupilWPosRH, pupilWPosLH;
//	pupilWPosLH = pupilWPosL  * _worldTohead;
//	pupilWPosRH = pupilWPosR  * _worldTohead;
//
//	//-----------------------------------------------------------------------------------------------
//	//Calculate Alfa to 0,0
//	osg::Vec3d 		   tgtW(0, 0, 0);
//	osg::Vec3d angleL, angleR, naL, naR;
//	angleL = getAngleBetween(pupilWPosL, tgtWPos);
//	angleR = getAngleBetween(pupilWPosR, tgtWPos);
//	naL = N2A(pupilWPosLN);
//	naR = N2A(pupilWPosRN);
//	double t = -pupilWPosL.z() / pupilWPosLN.z();
//	double wx = -(t* pupilWPosLN.x()) + pupilWPosL.x();
//	double wy = t* pupilWPosLN.y() + pupilWPosL.y();
//
//
//
//	osg::Matrixd leftM = getRotmatrix(angleL);
//	osg::Matrixd rightM = getRotmatrix(angleR);
//	osg::Matrixd headR = getRotmatrix(angles);
//	osg::Matrixd headRI;
//	headRI.invert(headR);
//
//	osg::Matrix eyeLeft = headRI*leftM;
//	osg::Matrix eyeRigth = headRI*rightM;
//
//	osg::Vec3d eyeLeftDeg = Utilities::MatrixGetRotation(eyeLeft);
//	osg::Vec3d eyeRightDeg = Utilities::MatrixGetRotation(eyeRigth);
//
//	osg::Vec3d eyeLeftDeg2 = Utilities::MatrixGetRotation(leftM*headRI);
//	osg::Vec3d eyeRightDeg2 = Utilities::MatrixGetRotation(rightM*headRI);
//
//	//-------------------------------------------------------------------------------------------------
//	//calculate center
//	osg::Vec3d cl, cr, hcl, hcr;
//
//	//refine
//	hcl = _leftEyeCenter;
//	hcr = _rightEyeCenter;
//
//	//hcl.z() += 8.0;
//	//hcr.z() += 8.0;
//	//---x-----
//	osg::Vec3d delta_L = pupilWPosLH - hcl;
//	//delta_L.z() = Zeye;
//
//	//delta_L.z() = delta_L.x() / tan(osg::DegreesToRadians(eyeLeftDeg.x())); //x/z = tan alfa   //angleL
//	//delta_L.y() += delta_L.z()*tan(osg::DegreesToRadians(-eyeLeftDeg.y()));
//	double tlz = delta_L.x() / tan(osg::DegreesToRadians(eyeLeftDeg.x())); //x/z = tan alfa   //angleL
//	
//	
//	double tly = delta_L.z()*tan(osg::DegreesToRadians(eyeLeftDeg.y()));
//	delta_L.y() = delta_L.y() - tly;
//	delta_L.z() = delta_L.z() - tlz;
//
//	osg::Vec3d delta_R = pupilWPosRH - hcr;
//
//	double trz = delta_R.x() / tan(osg::DegreesToRadians(eyeRightDeg.x())); //x/z = tan alfa
//	delta_R.y() -= delta_R.z() *tan(osg::DegreesToRadians(eyeRightDeg.y()));
//	delta_R.z() -= trz;
//
//	if (abs(delta_R.x()) > 100 || abs(delta_L.x()) > 100)
//		int jouhvibk = 67;
//
////	if (delta_L.z() > 0 && delta_L.y() > 0)
//	{
//		LeftCenterFixMap[LeftCenterFixMap.size()] = delta_L;
//		LCenterFixMap[LCenterFixMap.size()] = delta_L;
//	}
//
////	if (delta_R.z() > 0 && delta_R.y() > 0)
//	{
//		RightCenterFixMap[RightCenterFixMap.size()] = delta_R;
//		RCenterFixMap[RCenterFixMap.size()] = delta_R;
//	}
//
//
//	//--------------------------------------------------------------------------------------------------
//	//reconstruct
//	//
//	//-------------------------------------------------------------------------------------------------
//	//intersection
//	return true;
//
//}

bool AnotherCalibration(cv::Mat& colorImage, const PXCFaceData::LandmarksData* landmarkData, int counter, PXCCapture::Sample *sample, bool left, osg::Vec3d eyeCenter,
	osg::Vec3d position, osg::Vec3d angles, osg::Vec3d tgtWPos, cv::Point tgtImagePos, std::string title, osg::Vec3d& idealAim, osg::Vec3d& Aim)
{
	if (leftPupilFile2 == NULL)
		leftPupilFile2 = fopen("leftPupilFile2.txt", "w");
	if (rigthPupilFile2 == NULL)
		rigthPupilFile2 = fopen("rigthPupilFile2.txt", "w");
	double Zeye = 11.0;
	cv::circle(colorImage, tgtImagePos, 5, cv::Scalar(0, 0, 255), -1);
	//take pupil
	osg::Vec3d pupilWPosR, pupilWPosL;
	osg::Vec3d pupilWPosRN, pupilWPosLN;
	bool resL = getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_LEFT_CENTER, sample, pupilWPosLN, pupilWPosL);
	bool resR = getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_RIGHT_CENTER, sample, pupilWPosRN, pupilWPosR);

	if (!resL || !resR)
		return false;
	//--------------------------------------------------------------------------------------------
	if ((pupilWPosL.x() == pupilWPosL.y() && pupilWPosL.x() == pupilWPosL.z() && pupilWPosL.x() == 0) ||
		(pupilWPosR.x() == pupilWPosR.y() && pupilWPosR.x() == pupilWPosR.z() && pupilWPosR.x() == 0))
		return false;

	osg::Matrixd   _worldTohead, rMatrix, _headToWorld;
	_worldTohead = Utilities::worldToHead(position, angles);
	_headToWorld = Utilities::headToWorld(position, angles);

	osg::Vec3d	pupilWPosRH, pupilWPosLH;
	//pupilWPosLH = pupilWPosL  * _worldTohead;
	//pupilWPosRH = pupilWPosR  * _worldTohead;

	osg::Vec3d tempL = pupilWPosL - position;
	osg::Vec3d tempR = pupilWPosR - position;
	osg::Vec3d tempTgt = tgtWPos - position;
	osg::Matrixd tm = getRotmatrix(angles);
	osg::Matrixd Itm;
	Itm.invert(tm);

	pupilWPosLH = Itm * tempL;//pupilWPosL  * _worldTohead;
	pupilWPosRH = Itm *tempR;//pupilWPosR  * _worldTohead;
	osg::Vec3d	pupilWPosLH2 = tempL *Itm;//pupilWPosL  * _worldTohead;
	osg::Vec3d pupilWPosRH2 = tempR *Itm;

	fprintf(leftPupilFile2, "%f %f %f %f %f %f \n", pupilWPosL.x(), pupilWPosL.y(), pupilWPosL.z(), pupilWPosLH.x(), pupilWPosLH.y(), pupilWPosLH.z());
	fflush( leftPupilFile2 );
	fprintf(rigthPupilFile2, "%f %f %f %f %f %f \n", pupilWPosR.x(), pupilWPosR.y(), pupilWPosR.z(), pupilWPosRH.x(), pupilWPosRH.y(), pupilWPosRH.z());
	fflush(rigthPupilFile2 );

	osg::Vec3d 	   tgtHPos = Itm * tempTgt;//tgtWPos *	 _worldTohead;


	frame los(tgtHPos, pupilWPosLH, pupilWPosRH, tgtWPos, pupilWPosL, pupilWPosR);
	losList[losList.size()] = los;


	osg::Vec3d xx = _worldTohead	* pupilWPosR;
	//-----------------------------------------------------------------------------------------------
	//Calculate Alfa to 0,0
	osg::Vec3d 		   tgtW(0,0,0);
	osg::Vec3d angleL, angleR ,naL,naR;
	angleL = getAngleBetween(pupilWPosL, tgtWPos);
	angleR = getAngleBetween(pupilWPosR, tgtWPos);
	naL = N2A(pupilWPosLN);
	naR = N2A(pupilWPosRN);
	double t = -pupilWPosL.z() / pupilWPosLN.z();
	double wx = -(t* pupilWPosLN.x()) + pupilWPosL.x();
	double wy = t* pupilWPosLN.y() + pupilWPosL.y();



	osg::Matrixd leftM = getRotmatrix(angleL);
	osg::Matrixd rightM = getRotmatrix(angleR);
	osg::Matrixd headR = getRotmatrix(angles);
	osg::Matrixd headRI;
	headRI.invert(headR);

	osg::Matrix eyeLeft = headRI*leftM;
	osg::Matrix eyeRigth = headRI*rightM;

	osg::Vec3d eyeLeftDeg = Utilities::MatrixGetRotation(eyeLeft);
	osg::Vec3d eyeRightDeg = Utilities::MatrixGetRotation(eyeRigth);

	osg::Vec3d eyeLeftDeg2 = Utilities::MatrixGetRotation(leftM*headRI);
	osg::Vec3d eyeRightDeg2 = Utilities::MatrixGetRotation(rightM*headRI);

	//-------------------------------------------------------------------------------------------------
	//calculate center
	osg::Vec3d cl, cr, hcl, hcr, hcl2, hcr2;
	if (!GetEyeCenter(landmarkData, sample, cl, cr))
		return false;
	SaveLiDD(landmarkData, sample, position, angles);
	osg::Matrixd xxx = getRotmatrix(angles);
	osg::Matrixd xxI;
	xxI.invert(xxx);
	//refine
	hcl = xxI*(cl - position) ;
	hcr = xxI * (cr - position);
	//hcl2 = xxI * (cl - position);
	//hcr2 = xxI * (cr - position);

	//---x-----
	osg::Vec3d delta_L = pupilWPosLH - hcl;
	delta_L.z() = Zeye;
	//delta_L.z() = delta_L.x() / tan(osg::DegreesToRadians(eyeLeftDeg.x())); //x/z = tan alfa   //angleL
	//delta_L.y() += delta_L.z()*tan(osg::DegreesToRadians(-eyeLeftDeg.y()));
	double tlx = delta_L.z() * tan(osg::DegreesToRadians(eyeLeftDeg.x())); //x/z = tan alfa   //angleL
	double tly = delta_L.z()*tan(osg::DegreesToRadians(eyeLeftDeg.y()));	
	delta_L.x() = delta_L.x() - tlx;
	delta_L.y() = delta_L.y() - tly;
	
	osg::Vec3d delta_R = pupilWPosRH - hcr;
	delta_R.z() = Zeye;
	delta_R.x() -= delta_R.z() * tan(osg::DegreesToRadians(eyeRightDeg.x())); //x/z = tan alfa
	delta_R.y() -= delta_R.z() *tan(osg::DegreesToRadians(eyeRightDeg.y()));
	
	if (abs(delta_R.x()) > 100 || abs(delta_L.x()) > 100)
		int jouhvibk = 67;

	if (delta_L.z() > 0 && delta_L.y() > 0)
	{
		LeftCenterFixMap[LeftCenterFixMap.size()] = delta_L;
		LCenterFixMap[LCenterFixMap.size()] = delta_L;
	}
	else
		int jouhvibk = 67;
	if (delta_R.z() > 0 && delta_R.y() > 0)
	{
		RightCenterFixMap[RightCenterFixMap.size()] = delta_R;
		RCenterFixMap[RCenterFixMap.size()] = delta_R;
	}
	else
		int jouhvibk = 67;

	//--------------------------------------------------------------------------------------------------
	//reconstruct
	//
	//-------------------------------------------------------------------------------------------------
	//intersection
	return true;

}
bool EyeTrack(bool rawData,cv::Mat& colorImage, const PXCFaceData::LandmarksData* landmarkData, int counter, PXCCapture::Sample *sample, /*osg::Vec3d eyeCenter,*/
	osg::Vec3d position, osg::Vec3d angles, cv::Point tgtImagePos, std::string title, osg::Vec2d& AimL, osg::Vec2d& AimR, osg::Vec2d& nose)
{
	osg::Vec2d leftPupil,rightPupil;
	bool blp = getLandmarkPixel(landmarkData, PXCFaceData::LANDMARK_EYE_LEFT_CENTER, sample, leftPupil);
	bool brp = getLandmarkPixel(landmarkData, PXCFaceData::LANDMARK_EYE_RIGHT_CENTER, sample, rightPupil);
	bool bnose = getLandmarkPixel(landmarkData, PXCFaceData::LANDMARK_NOSE_TOP, sample, nose);
	if (rawData){
		if (blp ){//&& brp){
			AimL = leftPupil;
			//AimR = rightPupil;

		}
	}
	else{	//convert from camera pixels World to Screen
	}
	if (bnose && blp)
		return true;
	else
		return false;
	return true;
}

bool lineIntersec(osg::Vec3d p0, osg::Vec3d p1, double z, osg::Vec3d& intersection);
/* This is the main function calculate where are the eyes loking
Input:
cv::Mat& colorImage								: image to be drawed
const PXCFaceData::LandmarksData* landmarkData	:landmark
int counter										: not used
PXCCapture::Sample *sample						: reasense sample,
osg::Vec3d position								: head position
osg::Vec3d angles								: head orientation(degrees)
osg::Vec3d tgtWPos								: tgt world position (not used)(only for debug)
cv::Point tgtImagePos							: tgt image  position (not used)(only for debug)
std::string title								: image title

Output			:
osg::Vec3d& AimL								: left eye Aim in world coordinates
osg::Vec3d& AimR								: right eye Aim in world coordinates
*/
bool Sight(cv::Mat& colorImage, const PXCFaceData::LandmarksData* landmarkData, int counter, PXCCapture::Sample *sample, /*osg::Vec3d eyeCenter,*/
	osg::Vec3d position, osg::Vec3d angles, osg::Vec3d tgtWPos, cv::Point tgtImagePos, std::string title, osg::Vec3d& AimL, osg::Vec3d& AimR)
{
	if (leftPupilFile == NULL)
	{
		leftPupilFile = fopen("leftPupilFile.txt", "w");
	}
	if (rigthPupilFile == NULL)
	{
		rigthPupilFile = fopen("rigthPupilFile.txt", "w");
	}
	if (headPosFile == NULL)
	{
		headPosFile = fopen("headPosFile.txt", "w");
	}
	fprintf(headPosFile, "%lf %lf %lf %lf %lf %lf\n", position.x(), position.y(), position.z(), angles.x(), angles.y(), angles.z());
	fflush(rigthPupilFile);
	
	//take pupil
	osg::Vec3d pupilWPosR, pupilWPosL,normal;
	bool resLL = getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_LEFT_CENTER, sample, normal, pupilWPosL);
	bool resRR = getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_RIGHT_CENTER, sample, normal, pupilWPosR);
	if (!resLL || !resRR)
		return false;
	//PXCPointF32 point;
	//point.x = pupilWPosL.x();
	//point.y = pupilWPosL.y();
	//

	if ((pupilWPosL.x() == pupilWPosL.y() && pupilWPosL.x() == pupilWPosL.z() && pupilWPosL.x() == 0) ||
		(pupilWPosR.x() == pupilWPosR.y() && pupilWPosR.x() == pupilWPosR.z() && pupilWPosR.x() == 0))
		return false;
	//osg::Vec3d desiredL = getAngleBetween(pupilWPosL, tgtWPos);
	//osg::Vec3d desiredR = getAngleBetween(pupilWPosR, tgtWPos);
	//--------------------------------------------------------------------------------------------
	osg::Matrixd   _worldTohead, rMatrix, _headToWorld;
	
	osg::Matrixd tt = getRotmatrix(angles);
	osg::Matrixd tti;
	tti.invert(tt);
	_worldTohead = Utilities::worldToHead(position, angles);
	_headToWorld = Utilities::headToWorld(position, angles);

	osg::Vec3d	pupilWPosRH, pupilWPosLH;
	pupilWPosLH = tti *(pupilWPosL - position) ;// *_worldTohead;
	pupilWPosRH = tti*(pupilWPosR - position);// *_worldTohead;

	osg::Vec3d	pupilWPosRH2, pupilWPosLH2;

	osg::Vec3d check =  (tt *pupilWPosLH  )+position;



	fprintf(leftPupilFile, "%lf %lf %lf %lf %lf %lf\n", pupilWPosL.x(), pupilWPosL.y(), pupilWPosL.z(), pupilWPosLH.x(), pupilWPosLH.y(), pupilWPosLH.z());
	fflush(leftPupilFile); 
	fprintf(rigthPupilFile, "%lf %lf %lf %lf %lf %lf\n", pupilWPosR.x(), pupilWPosR.y(), pupilWPosR.z(), pupilWPosRH.x(), pupilWPosRH.y(), pupilWPosRH.z());
	fflush(rigthPupilFile);
	//-------------------------------------------------------------------------------------------------
	//calculate center
	osg::Vec3d cl, cr, hcl, hcr , clW,crW;
	if (!GetEyeCenter(landmarkData, sample, cl, cr))
		return false;

	 //Add delta from calibration file
	hcl = tti*(cl - position);
	hcr = tti*(cr - position);
	hcl.z() += ZLavg;
	hcr.z() += ZRavg;

	hcr.x() += XRavg;
	hcl.x() += XLavg;

	hcl.y() += YLavg;
	hcr.y() += YRavg;
	//calculate angle----
	//x / z = tan alfa
	osg::Vec3d angleL, angleR;
	osg::Vec3d angleL2, angleR2;

	double xl = pupilWPosLH.x() - hcl.x();
	double zl = -8.0;// pupilWPosLH.z() - hcl.z();
	double yl = pupilWPosLH.y() - hcl.y();

	//change heading direction
	angleL.x() =-( osg::RadiansToDegrees(atan2( zl,xl))+90.0)*1.20;
	angleL.y() = (osg::RadiansToDegrees(atan2((zl+2.5), yl)) + 90.0)*1.10+1;
	
	angleL2 = getAngleBetween(pupilWPosL, osg::Vec3d(0.0, 0.0, 0.0));

	double xr = pupilWPosRH.x() - hcr.x();
	double zr = -8.0;// pupilWPosRH.z() - hcr.z();
	double yr = pupilWPosRH.y() - hcr.y();
	//head coordinates
	angleR.x() = -(osg::RadiansToDegrees(atan2( zr,xr))+90.0)*1.20+1;
	angleR.y() = (osg::RadiansToDegrees(atan2(((zr+2.50)), yr)) + 90.0)*1.10+1;
	angleR2 = getAngleBetween(hcr, pupilWPosRH);
	osg::Vec3d angleR3 = getAngleBetween(pupilWPosR ,osg::Vec3d(0,0,0));
	//----------------------------------------------
	//	intersect
	osg::Vec3d segment(0, 0, -1000.0);
	//angles.x() = 0.0;
	//angles.y() = 0.0;
	//angles.z() = 30.0;
	//angleL.x() = -2.0;
	//angleL.y() = 0.0;
	//angleL.z() = 0.0;
	//angles.z() = -angles.z();

	osg::Matrixd headRot = getRotmatrix(angles);
	osg::Matrixd eyeLA = getRotmatrix(angleL);
	osg::Vec3d secondPL = segment * (headRot * eyeLA);
	
	osg::Vec3d secondOpinion = segment * (eyeLA * headRot);		 //
	//osg::Matrixd eyeLAi;
	//eyeLAi.invert(eyeLA);
	//osg::Vec3d faLjk = secondOpinion *eyeLAi ;
	//osg::Vec3d check1 = Utilities::MatrixGetRotation(headRot * eyeLA);
	//osg::Vec3d check2 = Utilities::MatrixGetRotation(eyeLA * headRot);
	secondPL = secondPL + pupilWPosL;

	osg::Vec3d resL = computeTerrainIntersection(scene, pupilWPosL, secondPL);

	AimL = resL;
	aimLFilter.add(AimL);
	AimL = aimLFilter.get();

	osg::Matrixd eyeRA = getRotmatrix(angleR);
	osg::Vec3d secondPR = segment * (headRot * eyeRA);
	
	osg::Vec3d secondOpinion2 = segment * (eyeRA * headRot);		   //
	////add the pupil and intersect

	secondPR = secondPR + pupilWPosR;
	osg::Vec3d resR = computeTerrainIntersection(scene, pupilWPosR, secondPR);

	AimR = resR;
	aimRFilter.add(AimR);
	AimR = aimRFilter.get();


	//------------------------------------------------------------
	if (abs(AimR.z()) == 0 || abs(AimL.z()) == 0)
		return false;
	return true;
}
osg::Vec2d EstimatebyPixel(osg::Vec2d data,Cal cal,bool left)
{
	osg::Vec2d ang;
	int _left=0;  //center
	int _up = 0;  //midle
	if (left)
	{
		if (data.x() > cal.upCenter.y() && data.x() > cal.midleCenter.y() && data.x() > cal.downCenter.y())
		{
			_left = 1;  //left
		}
		else
		{
			if (data.x() < cal.upCenter.y() && data.x() < cal.midleCenter.y() && data.x() < cal.downCenter.y())
			{
				_left = 2;	 //right
			}
		}

		if (data.y() < cal.midleCenter.w() && data.y() < cal.midleLeft.w() && data.y() < cal.midleRight.w())
		{
			_up = 1;  //up
		}
		else
		{
			if (data.y() > cal.midleCenter.w() && data.y() > cal.midleLeft.w() && data.y() > cal.midleRight.w())
			{
				_up = 2;	 //down
			}
		}
		//--------------
		if (_left == 1 && _up == 1)
		{
			double alfaH1 = (data.x() - cal.midleCenter.y()) / (cal.midleLeft.y() - cal.midleCenter.y());
			double h1 = alfaH1 * cal.midleLeft.x() + (1.0 - alfaH1)*  cal.midleCenter.x();
			//double p1 = alfaH1 * leftCal.midleLeft.w() + (1.0 - alfaH1)*  leftCal.midleCenter.w();

			double alfaH2 = (data.x() - cal.upCenter.y()) / (cal.upLeft.y() - cal.upCenter.y());
			double h2 = alfaH2 * cal.upLeft.x() + (1.0 - alfaH2)*  cal.upCenter.x();
			//double p2 = alfaH2 * leftCal.upLeft.w() + (1.0 - alfaH2)*  leftCal.upCenter.w();
			ang.x() = (h1 + h2) / 2.0;

			double alfaP1 = (data.y() - cal.midleCenter.w()) / (cal.upCenter.w() - cal.midleCenter.w());
			if (alfaP1 > 1)	 alfaP1 = 1.0;
			double p1 = alfaP1 * cal.upCenter.z() + (1.0 - alfaP1)*  cal.midleCenter.z();
			//double p1 = alfaH1 * leftCal.midleLeft.w() + (1.0 - alfaH1)*  leftCal.midleCenter.w();

			double alfaP2 = (data.y() - cal.midleLeft.w()) / (cal.upLeft.w() - cal.midleLeft.w());
			if (alfaP2 > 1)	 alfaP2 = 1.0;
			double p2 = alfaP2 * cal.upLeft.z() + (1.0 - alfaP2)*  cal.midleLeft.z();
			//double p2 = alfaH2 * leftCal.upLeft.w() + (1.0 - alfaH2)*  leftCal.upCenter.w();
			ang.y() = (p1 + p2) / 2.0;






		}
	}
	else
	{

		if (data.x() > cal.upCenter.y() && data.x() > cal.midleCenter.y() && data.x() > cal.downCenter.y())
		{
			_left = 1;  //left
		}
		else
		{
			if (data.x() < cal.upCenter.y() && data.x() < cal.midleCenter.y() && data.x() < cal.downCenter.y())
			{
				_left = 2;	 //right
			}
		}

		if (data.y() < cal.midleCenter.w() && data.y() < cal.midleLeft.w() && data.y() < cal.midleRight.w())
		{
			_up = 1;  //up
		}
		else
		{
			if (data.y() > cal.midleCenter.w() && data.y() > cal.midleLeft.w() && data.y() > cal.midleRight.w())
			{
				_up = 2;	 //down
			}
		}
		//--------------
		if (_left == 1 && _up == 1)
		{
			double alfaH1 = (data.x() - cal.midleCenter.y()) / (cal.midleLeft.y() - cal.midleCenter.y());
			double h1 = alfaH1 * cal.midleLeft.x() + (1.0 - alfaH1)*  cal.midleCenter.x();
			//double p1 = alfaH1 * leftCal.midleLeft.w() + (1.0 - alfaH1)*  leftCal.midleCenter.w();

			double alfaH2 = (data.x() - cal.upCenter.y()) / (cal.upLeft.y() - cal.upCenter.y());
			double h2 = alfaH2 * cal.upLeft.x() + (1.0 - alfaH2)*  cal.upCenter.x();
			//double p2 = alfaH2 * leftCal.upLeft.w() + (1.0 - alfaH2)*  leftCal.upCenter.w();
			ang.x() = (h1 + h2) / 2.0;

			double alfaP1 = (data.y() - cal.midleCenter.w()) / (cal.midleLeft.w() - cal.midleCenter.w());
			if (alfaP1 > 1)	 alfaP1 = 1.0;
			double p1 = alfaP1 * cal.midleLeft.z() + (1.0 - alfaP1)*  cal.midleCenter.z();
			//double p1 = alfaH1 * leftCal.midleLeft.w() + (1.0 - alfaH1)*  leftCal.midleCenter.w();

			double alfaP2 = (data.y() - cal.upCenter.w()) / (cal.upLeft.w() - cal.upCenter.w());
			if (alfaP2 > 1)	 alfaP2 = 1.0;
			double p2 = alfaP2 * cal.upLeft.z() + (1.0 - alfaP2)*  cal.upCenter.z();
			//double p2 = alfaH2 * leftCal.upLeft.w() + (1.0 - alfaH2)*  leftCal.upCenter.w();
			ang.y() = (p1 + p2) / 2.0;






		}
	}
		return ang;

}

void video(cv::Mat& img)
{
	//cv::VideoCapture in_capture("XRavg_%d.jpg");
	//cv::Mat img;


	//cv::VideoWriter out_capture("video.avi", CV_FOURCC('M', 'J', 'P', 'G'), 30, cv::Size(800, 600));

	//while (true)
	//{
		//in_capture >> img;
		//if (img.empty())
		//	break;

	if (!out_capture.isOpened())
	{
		std::cout << "!!! Output video could not be opened" << std::endl;
		return;
	}

		out_capture.write(img);
//	}
}
void video(void)
{
	cv::VideoCapture in_capture("XRavg_%d.jpg");
	cv::Mat img;


	cv::VideoWriter out_capture("video.avi", CV_FOURCC('M', 'J', 'P', 'G'), 30, cv::Size(800, 600));

	while (true)
	{
		in_capture >> img;
		if (img.empty())
			break;

		out_capture.write(img);
	}
}
bool SightWithNormal(cv::Mat& colorImage, const PXCFaceData::LandmarksData* landmarkData, int counter, PXCCapture::Sample *sample, /*osg::Vec3d eyeCenter,*/
	osg::Vec3d position, osg::Vec3d angles, osg::Vec3d tgtWPos, cv::Point tgtImagePos, std::string title, osg::Vec3d& AimL, osg::Vec3d& AimR)
{
	if (leftPupilFile == NULL)
	{
		leftPupilFile = fopen("leftPupilFile.txt", "w");
	}
	if (rigthPupilFile == NULL)
	{
		rigthPupilFile = fopen("rigthPupilFile.txt", "w");
	}
	if (headPosFile == NULL)
	{
		headPosFile = fopen("headPosFile.txt", "w");
	}
	fprintf(headPosFile, "%lf %lf %lf %lf %lf %lf\n", position.x(), position.y(), position.z(), angles.x(), angles.y(), angles.z());
	fflush(rigthPupilFile);
	//take pupil
	osg::Vec3d pupilWPosR, pupilWPosL;
	osg::Vec3d pupilWPosRN, pupilWPosLN;
	bool resLL = getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_LEFT_CENTER, sample, pupilWPosLN, pupilWPosL);
	bool resRR = getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_RIGHT_CENTER, sample, pupilWPosRN, pupilWPosR);
	if (!resLL || !resRR)
		return false;
	if ((pupilWPosL.x() == pupilWPosL.y() && pupilWPosL.x() == pupilWPosL.z() && pupilWPosL.x() == 0) ||
		(pupilWPosR.x() == pupilWPosR.y() && pupilWPosR.x() == pupilWPosR.z() && pupilWPosR.x() == 0))
		return false;
	//nL.add(pupilWPosLN);
	//pupilWPosLN  = nL.get();

	//nR.add(pupilWPosRN);
	//pupilWPosRN = nR.get();

	//osg::Vec3d desiredL = getAngleBetween(pupilWPosL, tgtWPos);
	//osg::Vec3d desiredR = getAngleBetween(pupilWPosR, tgtWPos);
	//--------------------------------------------------------------------------------------------
	osg::Matrix headRoll = getRotmatrix(osg::Vec3d(0, 0, angles.z()));
	osg::Vec2d leftHP = Utilities::DirectionToHP(pupilWPosLN);
	
	osg::Vec2d rightHP = Utilities::DirectionToHP(pupilWPosRN);

	osg::Vec3d la = getAngleBetween(pupilWPosL, tgtWPos);
	osg::Vec3d ra = getAngleBetween(pupilWPosR, tgtWPos);
	//lAngle.add(osg::Vec3d(leftHP.x(), leftHP.y(), 0));
	//rAngle.add(osg::Vec3d(rightHP.x(), rightHP.y(), 0));
	//osg::Vec3d tempL = lAngle.get();
	//osg::Vec3d tempR = rAngle.get();

	osg::Vec3d segment(0, 0, -1000.0);
	osg::Matrixd eyeLA = getRotmatrix(osg::Vec3d(-(180-leftHP.x()),- leftHP.y(), 0));
	osg::Vec3d secondPL = segment * ( eyeLA);
	//second opinion
	osg::Vec2d leftHP2 = Utilities::DirectionToHP(pupilWPosLN, headRoll);
	osg::Vec2d rightHP2 = Utilities::DirectionToHP(pupilWPosRN, headRoll);
	osg::Matrixd eyeLA2 = getRotmatrix(osg::Vec3d(leftHP2.x(), leftHP2.y(), 0));
	osg::Vec3d secondPL2 = segment * (eyeLA);

	//osg::Vec3d faL = MatrixGetRotation( eyeLA);
	//osg::Vec3d secondOpinion = segment * (eyeLA * headRot);
	//osg::Vec3d check1 = MatrixGetRotation(headRot * eyeLA);
	//osg::Vec3d check2 = MatrixGetRotation(eyeLA * headRot);
	secondPL = secondPL + pupilWPosL;

	osg::Vec3d resL = computeTerrainIntersection(scene, pupilWPosL, secondPL);
	osg::Vec3d intL,intR;
	lineIntersecDir(pupilWPosL, pupilWPosLN, -1.0, intL);
	AimL = intL;
	aimLFilter.add(AimL);
	AimL = aimLFilter.get();

	osg::Matrixd eyeRA = getRotmatrix(osg::Vec3d(-(180 - rightHP.x()), -rightHP.y(), 0));
	osg::Vec3d secondPR = segment * ( eyeRA);
	//second opinion............
	osg::Matrixd eyeRA2 = getRotmatrix(osg::Vec3d(rightHP2.x(), rightHP2.y(), 0));
	osg::Vec3d secondPR2 = segment * (eyeRA);
	//osg::Vec3d faR = MatrixGetRotation(headRot * eyeRA);
	//osg::Vec3d secondOpinion2 = segment * (eyeRA * headRot);
	//add the pupil and intersect

	secondPR = secondPR + pupilWPosR;
	osg::Vec3d resR = computeTerrainIntersection(scene, pupilWPosR, secondPR);
	lineIntersecDir(pupilWPosR, pupilWPosRN, -1.0, intR);
	AimR = intR;// resR;
	aimRFilter.add(AimR);
	AimR = aimRFilter.get();
	if (abs(AimR.z()) == 0 && abs(AimL.z()) == 0)
		return false;
	return true;
}
bool PointCalibration(cv::Mat& colorImage, const PXCFaceData::LandmarksData* landmarkData, int counter, PXCCapture::Sample *sample, bool left, osg::Vec3d eyeCenter,
	osg::Vec3d position, osg::Vec3d angles, osg::Vec3d tgtWPos, cv::Point tgtImagePos, std::string title, osg::Vec3d& idealAim, osg::Vec3d& Aim)
{
	osg::Vec3d eyeLid_L, eyeLid_R, eyeLid_T, eyeLid_B;
	osg::Vec3d pupilWPos;
	osg::Vec3d eyeLid_LN, eyeLid_RN, eyeLid_TN, eyeLid_BN;
	osg::Vec3d pupilWPosN;
	//counter++;
	cv::putText(colorImage, "Put your head On Center ", cv::Point(200, 100), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
	cv::putText(colorImage, "Each time look at the the red CIRCLE ", cv::Point(200, 130), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
	cv::putText(colorImage, "Do not move and Press any key .", cv::Point(200, 160), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);

	cv::circle(colorImage, tgtImagePos, 5, cv::Scalar(0, 0, 255), -1);
	osg::Vec3d pupilCenter;
	if (left)
	{
		if (!getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_LEFT_CENTER, sample, pupilWPosN, pupilWPos) ||
			!getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_LEFT_LEFT, sample, eyeLid_LN, eyeLid_L) ||
			!getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_LEFT_RIGHT, sample, eyeLid_RN, eyeLid_R) ||
			!getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_LEFT_TOP, sample, eyeLid_TN, eyeLid_T) ||
			!getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_LEFT_BOTTOM, sample, eyeLid_BN, eyeLid_B))
			return false;
		pupilCenter = osg::Vec3d(eyeLid_B.x() + 0.25, 9.0 + eyeLid_B.y(), (eyeLid_T.z() + eyeLid_B.z()) / 2.0);
		//pupilCenter = eyeCenter;
		pupilFilterL.add(pupilWPos);
		pupilWPos = pupilFilterL.get();
		pupilCenterFilterL.add(pupilCenter);
		pupilCenter = pupilCenterFilterL.get();
	}
	else
	{
		if (!getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYE_RIGHT_CENTER, sample, pupilWPosN, pupilWPos) ||
			!getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_RIGHT_LEFT, sample, eyeLid_LN, eyeLid_L) ||
			!getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_RIGHT_RIGHT, sample, eyeLid_RN, eyeLid_R) ||
			!getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_RIGHT_TOP, sample, eyeLid_TN, eyeLid_T) ||
			!getLandMarkOnWorld(landmarkData, PXCFaceData::LANDMARK_EYELID_RIGHT_BOTTOM, sample, eyeLid_BN, eyeLid_B))
			return false;

		pupilCenter = osg::Vec3d(eyeLid_B.x() - 0.0, 4.5 + eyeLid_B.y(), (eyeLid_T.z() + eyeLid_B.z()) / 2.0);
		//pupilCenter = eyeCenter;
		pupilFilterR.add(pupilWPos);
		pupilWPos = pupilFilterR.get();

		pupilCenterFilterR.add(pupilCenter);
		pupilCenter = pupilCenterFilterR.get();

	}

	//osg::Vec3d pupilCenter = osg::Vec3d( eyeLid_B.x()-0.0 , 9.0 + eyeLid_B.y()  , (eyeLid_T.z() + eyeLid_B.z()) / 2.0);
	osg::Vec3d headAngle = angles;// (angles.yaw, angles.pitch, angles.roll);

	osg::Vec3d requiredRelativeAnglePupil = getRelativeAngle(tgtWPos, pupilWPos, headAngle);
	//osg::Vec3d requiredRelativeAngleCenterPupil = getRelativeAngle(tgtWPos, pupilCenter, headAngle);



	osg::Matrixd   _worldTohead, rMatrix, _headToWorld;
	double rx, ry, rz;
	_worldTohead = Utilities::worldToHead(position, angles);
	_headToWorld = Utilities::headToWorld(position, angles);

	//osg::Vec3d eyeCenterH = pupilCenter;// *_worldTohead;
	//osg::Vec3d eyeCenterV = pupilCenter;// *_worldTohead;
	osg::Vec3d eyeCenterH = pupilCenter *_worldTohead;
	osg::Vec3d eyeCenterV = pupilCenter *_worldTohead;

	eyeCenterH -= osg::Vec3d(0, 0, -10.0);
	eyeCenterV -= osg::Vec3d(0, 0, -9.0);

	osg::Vec3d pupilLeHeadpos = pupilWPos * 	_worldTohead;
	//osg::Vec3d pupilCenterLeHeadpos = pupilCenter * 	_worldTohead;



	osg::Vec3d eyeAngleH = getAngleBetween(eyeCenterH, pupilLeHeadpos);
	osg::Vec3d eyeAngleV = getAngleBetween(eyeCenterV, pupilLeHeadpos);
	//osg::Vec3d eyeAngleH2 = getAngleBetween(pupilCenter - osg::Vec3d(0, 0, -13.0), pupilWPos);
	//osg::Vec3d eyeAngleV2 = getAngleBetween(pupilCenter - osg::Vec3d(0, 0, -10.0), pupilWPos);
	//eyeAngleH.y() = eyeAngleV.y();

	osg::Vec3d segment(0, 0, -1000.0);
	//-----------------------------------------------------------
	osg::Vec3d alfa = getAngleBetween(pupilWPos, tgtWPos);
	osg::Matrixd	idealmatrix = getRotmatrix(alfa);
	osg::Vec3d secondP = segment*idealmatrix;
	secondP = osg::Vec3d(-secondP.x() + pupilWPos.x(), secondP.y() + pupilWPos.y(), secondP.z() + pupilWPos.z());
	osg::Vec3d res = computeTerrainIntersection(scene, pupilWPos, secondP);
		
	//---------------------------------------------------------------



	pCalculatedLeftEyeAngle[pCalculatedLeftEyeAngle.size()] = eyeAngleH;
	
	rMatrix = getRotmatrix(eyeAngleH);
	osg::Matrixd iMatrix = getRotmatrix(requiredRelativeAnglePupil);
	osg::Matrixd hrotate;
	osg::Vec3d opositeHead(-headAngle.x(), headAngle.y(), headAngle.z());
	hrotate = getRotmatrix(opositeHead);
	osg::Matrixd pp = rMatrix*hrotate;// hrotate;
	Utilities::MatrixGetRotation(pp, rz, rx, ry);
	osg::Matrixd pp2 = hrotate *rMatrix;// hrotate;
	double rx2, ry2, rz2;
	//----------------------
	Utilities::MatrixGetRotation(pp2, rz2, rx2, ry2);
	//----------------
	osg::Matrixd rpp = hrotate*iMatrix;
	Utilities::MatrixGetRotation(rpp, rz, rx, ry);
	osg::Vec3d PlanePoint = (segment*  pp2);

	PlanePoint = osg::Vec3d(-PlanePoint.x() + pupilWPos.x(), PlanePoint.y() + pupilWPos.y(), PlanePoint.z() + pupilWPos.z());

	osg::Vec3d reqpoint(segment * rpp);

	reqpoint = osg::Vec3d(+reqpoint.x() + pupilWPos.x(), reqpoint.y() + pupilWPos.y(), reqpoint.z() + pupilWPos.z());

	osg::Vec3d p= computeTerrainIntersection(scene, pupilWPos, PlanePoint);
	idealAim = computeTerrainIntersection(scene, pupilWPos, reqpoint);
	Aim = p;
	int i = title.compare("Left tgt1");
	osg::Vec3d pupilLeftAngle = getAngleBetween(pupilWPos, position, angles, tgtWPos);
	//osg::Vec3d pupilRightAngle = getAngleBetween(pupilRight, position, angles, tgtWPos);

	ppLDeg[ppLDeg.size()] = pupilLeftAngle;
	//ppRDeg[ppRDeg.size()] = pupilRightAngle;
	pcLDeg[pcLDeg.size()] = getAngleBetween(pupilCenter, position, angles, tgtWPos);



	if (counter > 1001)
	{
		FILE* ff = fopen("tgtAngle.txt", "a");
		if (ff != NULL)
		{
			fprintf(ff, "%s\n", title.c_str());
			fprintf(ff, "Pupil \n");

			osg::Vec3d sum(0, 0, 0);
			for each (auto var in ppLDeg)
				sum += var.second;
			if (ppLDeg.size() > 0)
				sum /= ppLDeg.size();
			fprintf(ff, " %f %f %f\n", sum.x(), sum.y(), sum.z());
			//---------------------------------------
			fprintf(ff, "calculated  Eye Angle \n");
			sum = osg::Vec3d(0, 0, 0);
			for each (auto var in pCalculatedLeftEyeAngle)
				sum += var.second;
			if (pCalculatedLeftEyeAngle.size() > 0)
				sum /= pCalculatedLeftEyeAngle.size();
			fprintf(ff, " %f %f %f\n", sum.x(), sum.y(), sum.z());
			//---------------------------------------
			sum = osg::Vec3d(0, 0, 0);
			fprintf(ff, "Pupil Center\n");
			for each (auto var in pcLDeg)
				sum += var.second;
			if (pcLDeg.size() > 0)
				sum /= pcLDeg.size();
			fprintf(ff, " %f %f %f\n", sum.x(), sum.y(), sum.z());

			//---------------------------------------
			//TODO :calculate  avg  pupil ,pupilcenter convert to eyecenter and caculate the angle between centereye to pupil
			//calculate the Angle dif
			//calculate radius by angle.
			fprintf(ff, "---------------------------------------------\n");
			//---------------------------------------
			fflush(ff);
			fclose(ff);
		}
		return true;
		
	}
	return false;
}
//goals
//calculate the eyes center in head coordinates

void ReadCalibratedPixelsMethod(char* name)
{
	//read file
	
	double ang ;
	double pix;
	//std::vector<cv::Point2d> pointsL;
	char _nameL[250];
	sprintf(_nameL, "%s_CalibrationLeft.txt", name);
	FILE* fleft = fopen(_nameL, "r");

	char _nameR[250];
	sprintf(_nameR, "%s_CalibrationRight.txt", name);
	FILE* fright = fopen(_nameR, "r");

	{
		double lha, lhp, lpa, lpp;
		//while (!feof(fleft))
		//{
			fscanf(fleft, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			leftCal.upLeft = osg::Vec4d(lha, lhp, lpa, lpp);

			fscanf(fleft, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			leftCal.upCenter = osg::Vec4d(lha, lhp, lpa, lpp);

			fscanf(fleft, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			leftCal.upRight = osg::Vec4d(lha, lhp, lpa, lpp);

			fscanf(fleft, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			leftCal.midleLeft = osg::Vec4d(lha, lhp, lpa, lpp);

			fscanf(fleft, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			leftCal.midleCenter = osg::Vec4d(lha, lhp, lpa, lpp);

			fscanf(fleft, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			leftCal.midleRight = osg::Vec4d(lha, lhp, lpa, lpp);

			fscanf(fleft, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			leftCal.downLeft = osg::Vec4d(lha, lhp, lpa, lpp);

			fscanf(fleft, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			leftCal.downCenter = osg::Vec4d(lha, lhp, lpa, lpp);

			fscanf(fleft, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			leftCal.downRight = osg::Vec4d(lha, lhp, lpa, lpp);
			//pointsL.push_back(cv::Point2d(ang,pix));
		//}
		fclose(fleft);
	}
		{
			double lha, lhp, lpa, lpp;
			//while (!feof(fleft))
			//{
			fscanf(fright, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			rightCal.upLeft = osg::Vec4d(lha, lhp, lpa, lpp);

			fscanf(fright, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			rightCal.upCenter = osg::Vec4d(lha, lhp, lpa, lpp);

			fscanf(fright, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			rightCal.upRight = osg::Vec4d(lha, lhp, lpa, lpp);

			fscanf(fright, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			rightCal.midleLeft = osg::Vec4d(lha, lhp, lpa, lpp);

			fscanf(fright, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			rightCal.midleCenter = osg::Vec4d(lha, lhp, lpa, lpp);

			fscanf(fright, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			rightCal.midleRight = osg::Vec4d(lha, lhp, lpa, lpp);

			fscanf(fright, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			rightCal.downLeft = osg::Vec4d(lha, lhp, lpa, lpp);

			fscanf(fright, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			rightCal.downCenter = osg::Vec4d(lha, lhp, lpa, lpp);

			fscanf(fright, "%lf  %lf %lf  %lf", &lha, &lhp, &lpa, &lpp);
			rightCal.downRight = osg::Vec4d(lha, lhp, lpa, lpp);
			//pointsL.push_back(cv::Point2d(ang,pix));
			//}
			fclose(fright);
		}
	//cv::Vec4f  line;
	//{

	//}
	//{
	//	FILE* flh = fopen("leftheading.txt", "r");
	//	while (!feof(flh))
	//	{
	//		fscanf(flh, "%lf  %lf ", &ang, &pix);
	//		leftHeadingMap[ang] = pix;
	//		//pointsL.push_back(cv::Point2d(ang,pix));
	//	}
	//	fclose(flh);
	//	int n = leftHeadingMap.size();
	//	cv::Point* points = new cv::Point[n];
	//	int i = 0;
	//	for each (auto pos in leftHeadingMap)
	//	{
	//		points[i].x = pos.first;
	//		points[i].y = pos.second;
	//		i++;
	//	}
	//	cv::Mat pointMat = cv::Mat(1, n, CV_32SC2, points);
	//	//float line[4];
	//	cv::fitLine(pointMat, line, CV_DIST_L2, 0, 0.001, 0.001);//pointsL
	//	leftHeading.m = (line[1] / line[0]);
	//	leftHeading.b = line[3] - leftHeading.m*line[2];
	//}
	////----------------------------
	//
	//	{
	//		FILE* frh = fopen("rightheading.txt", "r");
	//		while (!feof(frh))
	//		{
	//			fscanf(frh, "%lf  %lf ", &ang, &pix);
	//			rigthHeadingMap[ang] = pix;
	//			//pointsL.push_back(cv::Point2d(ang,pix));
	//		}
	//		fclose(frh);
	//		int n = rigthHeadingMap.size();
	//		cv::Point* points = new cv::Point[n];
	//		int i = 0;
	//		for each (auto pos in rigthHeadingMap)
	//		{
	//			points[i].x = pos.first;
	//			points[i].y = pos.second;
	//			i++;
	//		}
	//		cv::Mat pointMat = cv::Mat(1, n, CV_32SC2, points);
	//		//float line[4];
	//		cv::fitLine(pointMat, line, CV_DIST_L2, 0, 0.001, 0.001);//pointsL
	//		rightHeading.m = (line[1] / line[0]);
	//		rightHeading.b = line[3] - rightHeading.m*line[2];
	//	}
	////----------------------------

	//	{
	//		FILE* flp = fopen("leftpitch.txt", "r");
	//		while (!feof(flp))
	//		{
	//			fscanf(flp, "%lf  %lf ", &ang, &pix);
	//			leftPitchMap[ang] = pix;
	//			//pointsL.push_back(cv::Point2d(ang,pix));
	//		}
	//		fclose(flp);
	//		int n = leftPitchMap.size();
	//		cv::Point* points = new cv::Point[n];
	//		int i = 0;
	//		for each (auto pos in leftPitchMap)
	//		{
	//			points[i].x = pos.first;
	//			points[i].y = pos.second;
	//			i++;
	//		}
	//		cv::Mat pointMat = cv::Mat(1, n, CV_32SC2, points);
	//		//float line[4];
	//		cv::fitLine(pointMat, line, CV_DIST_L2, 0, 0.001, 0.001);//pointsL
	//		leftPitch.m = (line[1] / line[0]);
	//		leftPitch.b = line[3] - leftPitch.m*line[2];
	//	}
	////----------------------------

	//	{
	//		FILE* flp = fopen("rightpitch.txt", "r");
	//		while (!feof(flp))
	//		{
	//			fscanf(flp, "%lf  %lf ", &ang, &pix);
	//			rigthPitchMap[ang] = pix;
	//			//pointsL.push_back(cv::Point2d(ang,pix));
	//		}
	//		fclose(flp);
	//		int n = rigthPitchMap.size();
	//		cv::Point* points = new cv::Point[n];
	//		int i = 0;
	//		for each (auto pos in rigthPitchMap)
	//		{
	//			points[i].x = pos.first;
	//			points[i].y = pos.second;
	//			i++;
	//		}
	//		cv::Mat pointMat = cv::Mat(1, n, CV_32SC2, points);
	//		//float line[4];
	//		cv::fitLine(pointMat, line, CV_DIST_L2, 0, 0.001, 0.001);//pointsL
	//		rightPitch.m = (line[1] / line[0]);
	//		rightPitch.b = line[3] - rightPitch.m*line[2];
	//	}
	printf("hh");

}
void SaveEyePixelMap(char* name)
{
	//FILE* fPersonal = fopen(name, "w");
	//FILE* flh = fopen("leftheading.txt", "w");
	//FILE* frh = fopen("rightheading.txt", "w");
	//FILE* flp = fopen("leftpitch.txt", "w");
	//FILE* frp = fopen("rightpitch.txt", "w");

	char _nameL[250];
	sprintf(_nameL, "%s_CalibrationLeft.txt", name);
	FILE* fleft = fopen(_nameL, "w");

	char _nameR[250];
	sprintf(_nameR, "%s_CalibrationRight.txt", name);
	FILE* fright = fopen(_nameR, "w");
	//fprintf(fPersonal, "left heading \n");

	for each (auto pos in leftHeadingMap)
	{
		int key = pos.first;
		osg::Vec2d Lheading = pos.second;
		osg::Vec2d Lpitch = leftPitchMap[key];
		//fprintf(fPersonal, "Ang %lf = %lf \n", ang, pix);
		fprintf(fleft, " %lf  %lf  %lf  %lf \n", Lheading.x(), Lheading.y(), Lpitch.x(), Lpitch.y());
	}
	fclose(fleft);
	//fprintf(fPersonal, "right heading \n");

	for each (auto pos in rigthHeadingMap)
	{
		int key = pos.first;
		osg::Vec2d Rheading = pos.second;
		osg::Vec2d Rpitch = leftPitchMap[key];
		//fprintf(fPersonal, "Ang %lf = %lf \n", ang, pix);
		fprintf(fright, " %lf  %lf  %lf  %lf \n", Rheading.x(), Rheading.y(), Rpitch.x(), Rpitch.y());
	}
	fclose(fright);

	//fprintf(fPersonal, "left pitch \n");

	//for each (auto pos in leftPitchMap)
	//{
	//	double ang = pos.first;
	//	double pix = pos.second;
	//	fprintf(flp, " %lf  %lf \n", ang, pix);
	//}
	//fclose(flp);
	////fprintf(fPersonal, "right pitch \n");

	//for each (auto pos in rigthPitchMap)
	//{
	//	double ang = pos.first;
	//	double pix = pos.second;
	//	fprintf(frp, " %lf  %lf \n", ang, pix);
	//}
	//fclose(frp);
	//fflush(fPersonal);
	//fclose(fPersonal);


}
void SaveEyeCenter(char* name, std::map<int, osg::Vec2d> leftPixel, std::map<int, osg::Vec3d> leftAngle, std::map<int, osg::Vec2d> rightPixel, std::map<int, osg::Vec3d> rightAngle)
{
	double ZLavg = 0;
	double YLavg = 0;
	double XLavg = 0;
	double ZRavg = 0;
	double YRavg = 0;
	double XRavg = 0;
	for each (auto pos in leftAngle)
	{
		ZLavg += pos.second.z();
		XLavg += pos.second.x();
		YLavg += pos.second.y();
	}
	ZLavg /= leftAngle.size();
	YLavg /= leftAngle.size();
	XLavg /= leftAngle.size();


	for each (auto pos in rightAngle)
	{
		ZRavg += pos.second.z();
		YRavg += pos.second.y();
		XRavg += pos.second.x();
	}
	ZRavg /= rightAngle.size();
	YRavg /= rightAngle.size();
	XRavg /= rightAngle.size();


	double YLPavg = 0;
	double XLPavg = 0;
	double YRPavg = 0;
	double XRPavg = 0;
	for each (auto pos in leftPixel)
	{

		XLPavg += pos.second.x();
		YLPavg += pos.second.y();
	}

	YLPavg /= leftPixel.size();
	XLPavg /= leftPixel.size();


	for each (auto pos in rightPixel)
	{

		YRPavg += pos.second.y();
		XRPavg += pos.second.x();
	}

	YRPavg /= rightPixel.size();
	XRPavg /= rightPixel.size();


	FILE* fPersonal = fopen(name, "w");
	leftHeadingMap[leftHeadingMap.size()] = osg::Vec2d(XLavg, XLPavg);
	leftPitchMap[leftPitchMap.size()] = osg::Vec2d(YLavg, YLPavg);
	rigthHeadingMap[rigthHeadingMap.size()] = osg::Vec2d(XRavg, XRPavg);
	rigthPitchMap[rigthPitchMap.size()] = osg::Vec2d(YRavg, YRPavg);

	fprintf(fPersonal, "YLAng %lf = %lf \n", YLavg, YLPavg);
	fprintf(fPersonal, "XLAng %lf = %lf \n", XLavg, XLPavg);
	fprintf(fPersonal, "YRavg %lf = %lf \n", YRavg, YRPavg);
	fprintf(fPersonal, "XRavg %lf = %lf \n", XRavg, XRPavg);
	fflush(fPersonal);
	fclose(fPersonal);
}
// measure acurrancy

void SaveEyeCenter(char* name, std::map<int, osg::Vec3d> left, std::map<int, osg::Vec3d> right)
{
	double ZLavg = 0;
	double YLavg = 0;
	double XLavg = 0;
	double ZRavg = 0;
	double YRavg = 0;
	double XRavg = 0;
	for each (auto pos in left)
	{
		ZLavg += pos.second.z();
		XLavg += pos.second.x();
		YLavg += pos.second.y();
	}
	ZLavg /= left.size();
	YLavg /= left.size();
	XLavg /= left.size();


	for each (auto pos in right)
	{
		ZRavg += pos.second.z();
		YRavg += pos.second.y();
		XRavg += pos.second.x();
	}
	ZRavg /= right.size();
	YRavg /= right.size();
	XRavg /= right.size();


	FILE* fPersonal = fopen(name, "w");
	fprintf(fPersonal, "ZLavg %lf\n", ZLavg);
	fprintf(fPersonal, "YLavg %lf\n", YLavg);
	fprintf(fPersonal, "XLavg %lf\n", XLavg);
	fprintf(fPersonal, "ZRavg %lf\n", ZRavg);
	fprintf(fPersonal, "YRavg %lf\n", YRavg);
	fprintf(fPersonal, "XRavg %lf\n", XRavg);
	fflush(fPersonal);
	fclose(fPersonal);
}
void SaveLos()
{
	FILE* fPersonal = fopen("CalculateCenter.txt", "w");
	for each (auto raw in losList)
	{

		fprintf(fPersonal, " %f %f %f ", raw.second.p1H.x(), raw.second.p1H.y(), raw.second.p1H.z());
		fprintf(fPersonal, " %f %f %f ", raw.second.p2HL.x(), raw.second.p2HL.y(), raw.second.p2HL.z());
		fprintf(fPersonal, " %f %f %f ", raw.second.p2HR.x(), raw.second.p2HR.y(), raw.second.p2HR.z());
		fprintf(fPersonal, " %f %f %f ", raw.second.p1W.x(), raw.second.p1W.y(), raw.second.p1W.z());
		fprintf(fPersonal, " %f %f %f ", raw.second.p2WL.x(), raw.second.p2WL.y(), raw.second.p2WL.z());
		fprintf(fPersonal, " %f %f %f \n", raw.second.p2WR.x(), raw.second.p2WR.y(), raw.second.p2WR.z());
		fflush(fPersonal);
	}
	fclose(fPersonal);
}
void ReadLos()
{
	losList.clear();
	FILE* fPersonal = fopen("CalculateCenter.txt","r");
	while (!feof(fPersonal))
	{
		osg::Vec3d p1H;
		osg::Vec3d p2HL;
		osg::Vec3d p2HR;
		osg::Vec3d p1W;
		osg::Vec3d p2WL;
		osg::Vec3d p2WR;
		fscanf(fPersonal, " %lf %lf %lf ", &p1H.x(), &p1H.y(), &p1H.z());
		fscanf(fPersonal, " %lf %lf %lf ", &p2HL.x(), &p2HL.y(), &p2HL.z());
		fscanf(fPersonal, " %lf %lf %lf ", &p2HR.x(), &p2HR.y(), &p2HR.z());
		fscanf(fPersonal, " %lf %lf %lf ", &p1W.x(), &p1W.y(), &p1W.z());
		fscanf(fPersonal, " %lf %lf %lf ", &p2WL.x(), &p2WL.y(), &p2WL.z());
		fscanf(fPersonal, " %lf %lf %lf ", &p2WR.x(), &p2WR.y(), &p2WR.z());
		frame los(p1H, p2HL, p2HR, p1W, p2WL, p2WR);
		losList[losList.size()] = los;
	}
	fclose(fPersonal);
}
bool lineIntersecDir(osg::Vec3d p0, osg::Vec3d dir, double z, osg::Vec3d& intersection)
{

	double t;
	if (dir.z() != 0)
	{
		double t = (z - p0.z()) / dir.z();
		intersection = p0 + dir *t;
		return true;
	}
	return false;
}
bool lineIntersec(osg::Vec3d p0, osg::Vec3d p1, double z, osg::Vec3d& intersection)
{
	osg::Vec3d dir(p1 - p0);
	//z = los.p1H.z() + t *  dir.z();
	double t;
	if (dir.z() != 0)
	{
		double t = (z - p0.z()) / dir.z();
		intersection = p0 + dir *t;
		return true;
	}
	return false;
 }
double error(osg::Vec3d p1, osg::Vec3d p2)
{
	osg::Vec3d diff(p1-p2);
	return diff.length();
}
void ProcessCenter(std::string name)
{
	
	ReadLos();
	osg::Vec3d avgLH, avgLW, avgRH, avgRW, avgLHH;
	double zH = losList[0].p2HL.z() ;
	double zW = losList[0].p2WL.z() ;
	double MinerrorLH = 1000000;
	double MinerrorLW = 1000000;
	double MinerrorRH = 1000000;
	double MinerrorRW = 1000000;
	osg::Vec3d CLH, CLW, CRH, CRW;
	char nameLh[80];
	char nameRh[80];
	char nameLw[80];
	char nameRw[80];
	for (float i = 0; i < 20; i += 0.01)
	{
		cv::Mat lhI = cv::Mat(640, 640, CV_8UC1, cv::Scalar(0));
		cv::Mat rhI = cv::Mat(640, 640, CV_8UC1, cv::Scalar(0));
		cv::Mat lwI = cv::Mat(640, 640, CV_8UC1, cv::Scalar(0));
		cv::Mat rwI = cv::Mat(640, 640, CV_8UC1, cv::Scalar(0));
		
		sprintf(nameLh, "lh_%f.jpg", i);
		sprintf(nameRh, "rh_%f.jpg", i);
		sprintf(nameLw, "lw_%f.jpg", i);
		sprintf(nameRw, "rw_%f.jpg", i);
		for each (auto raw in losList)
		{
			auto los = raw.second;
			osg::Vec3d intersect;
			if (lineIntersec(los.p1H, los.p2HL, zH, intersect))
			{
				avgLH += intersect;
				cv::circle(lhI, cv::Point2d(intersect.x()*5.0 + 320, intersect.y()*5.0 + 320), 1, cv::Scalar(255, 255, 255));
					//cv::circle(colorImage, screenPoint, 20, cv::Scalar(0, 0, 255), -1);
			}
			if (lineIntersec(los.p1H, los.p2HR, zH, intersect))
				avgRH += intersect;

			if (lineIntersec(los.p1W, los.p2WL, zW, intersect))
				avgLW += intersect;
			if (lineIntersec(los.p1W, los.p2WR, zW, intersect))
				avgRW += intersect;
		}
		// may be another aprox.  to find the center
		avgLH /= losList.size();
		avgRH /= losList.size();
		avgLW /= losList.size();
		avgRW /= losList.size();
		cv::imwrite(nameLh, lhI);
		vector<cv::Vec3f> circles;
		cv::HoughCircles(lhI, circles, CV_HOUGH_GRADIENT, 1, lhI.rows / 2, 200, 5, 5, 50);	/// Draw the circles detected
		double errorLH = 0, errorLW = 0, errorRH = 0, errorRW = 0,errorLHH = 0 ;
		bool compare = false;
		if (circles.size() == 1)
		{
			avgLHH = osg::Vec3d((circles[0][0] - 320) / 5.0, (circles[0][1] - 320) / 5.0, zH);
			compare = true;
		}
		for each (auto raw in losList)
		{
			auto los = raw.second;
			osg::Vec3d intersect;
			if (lineIntersec(los.p1H, los.p2HL, zH, intersect))
			{
				errorLH += error(intersect, avgLH);
				errorLHH += error(intersect, avgLHH);
			}
			if (lineIntersec(los.p1H, los.p2HR, zH, intersect))
				errorRH += error(intersect, avgRH);

			if (lineIntersec(los.p1W, los.p2WL, zW, intersect))
				errorLW += error(intersect, avgLW);
			if (lineIntersec(los.p1W, los.p2WR, zW, intersect))
				errorRW += error(intersect, avgRW);
		}
		if (errorLH < MinerrorLH)
		{
			MinerrorLH = errorLH;
			CLH = avgLH;
		}
		if (errorRH < MinerrorRH)
		{
			MinerrorRH = errorRH;
			CRH = avgRH;
		}
		if (errorLW < MinerrorLW)
		{
			MinerrorLW = errorLW;
			CLW = avgLW;
		}
		if (errorRW < MinerrorRW)
		{
			MinerrorRW = errorRW;
			CRW = avgRW;
		}
		zH += 0.01;
		zW += 0.01;
	}
	FILE* fCenters = fopen(name.c_str(), "w");
	if (fCenters != NULL)
	{
		fprintf(fCenters, " %f %f %f \n", CLH.x(), CLH.y(), CLH.z());
		fprintf(fCenters, " %f %f %f \n", CRH.x(), CRH.y(), CRH.z());
		fprintf(fCenters, " %f %f %f \n", CLW.x(), CLW.y(), CLW.z());
		fprintf(fCenters, " %f %f %f \n", CRW.x(), CRW.y(), CRW.z());
		fflush(fCenters);
		fclose(fCenters);
	}
}
bool getCenters(std::string name, osg::Vec3d& centerLeft, osg::Vec3d& centerRight)
{
	FILE* fCenters = fopen(name.c_str(), "r");
	osg::Vec3d CLH, CLW, CRH, CRW;
	if (fCenters != NULL)
	{
		fscanf(fCenters, " %lf %lf %lf \n", &centerLeft.x(), &centerLeft.y(), &centerLeft.z());
		fscanf(fCenters, " %lf %lf %lf \n", &centerRight.x(), &centerRight.y(), &centerRight.z());
		fscanf(fCenters, " %lf %lf %lf \n", &CLW.x(), &CLW.y(), &CLW.z());
		fscanf(fCenters, " %lf %lf %lf \n", &CRW.x(), &CRW.y(), &CRW.z());
		
		fclose(fCenters);
		return true;
	}
	return false;
}
void TestCenter()
{
	ProcessCenter("ariel.txt");
}

void DrawHeadMarker(cv::Mat& colorImage, osg::Vec3d headPos, osg::Vec3d headDeg)
{
	cv::Size size = colorImage.size();

	cv::Point p1(size.width / 2, 230);
	cv::Point p2(size.width / 2, 350);
	cv::Point p3(50 + size.width / 2, 230);
	cv::Point p4(50 + size.width / 2, 350);
	cv::Point centerY(10.0*headDeg.x() + size.width / 2, 250);
	cv::Point centerR(10.0*headDeg.y() + size.width / 2, 250);
	cv::Point centerP(10.0*headDeg.z() + size.width / 2, 250);
	cv::Rect xH(-headPos.x() + size.width / 2, 250 + 40, 20, 20);
	//printf("Head Angle(%f,%f)\n", angles.yaw, angles.roll);
	cv::line(colorImage, p1, p2, cv::Scalar(0, 0, 255), 1);
	cv::line(colorImage, p3, p4, cv::Scalar(0, 0, 255), 1);
	cv::circle(colorImage, centerY, 10, cv::Scalar(0, 255, 255), 1);
	cv::circle(colorImage, centerR, 10, cv::Scalar(0, 255, 128), 1);
	cv::circle(colorImage, centerP, 10, cv::Scalar(255, 0, 128), 1);
	cv::rectangle(colorImage, xH, cv::Scalar(255, 0, 0), 1);
}
/* This is the main Step
Capture data from Realsense camera call Sight function receive the Aimed points and draw them
*/
void play1(char* name,PXCSenseManager* senseManager, PXCFaceData* m_output)
{
	bool stop = false;
	//----------------- define 	Black background -------------------------------
	cv::Mat colorImage = cv::Mat(600, 800, CV_8UC3);
	colorImage = cv::Scalar::all(0);
	osg::Vec3d aimLeft, aimRight;
	//bool firstTime = true;
	printf("Press any key to continue");
	int a =getch();
	int counter = 0;
	int row = 0;
	headPoseFilter.reset();
	headAngleFilter.reset();
	for (; !stop;)  //for Aligned Images
	{
		//-----------------capture Image----------------------------
		pxcStatus sts = senseManager->AcquireFrame(true);//blocking
		if (sts < PXC_STATUS_NO_ERROR)
			break;
		PXCCapture::Sample *sample = senseManager->QuerySample();
		//-----------------prepare depth map on world coordinates------------------
		createVerticesFromDepth(sample->depth);
		m_output->Update();
		const int numFaces = m_output->QueryNumberOfDetectedFaces();
		//--------------if find face------------------

		
		if (numFaces > 0)
		{
			PXCFaceData::Face* trackedFace = m_output->QueryFaceByIndex(0);
			//--------------------------pose----------------------------
			const PXCFaceData::PoseData* poseData = trackedFace->QueryPose();
			PXCFaceData::PoseEulerAngles angles;
			PXCFaceData::HeadPosition position;
			poseData->QueryPoseAngles(&angles);
			poseData->QueryHeadPosition(&position);
			const PXCFaceData::LandmarksData* landmarkData = trackedFace->QueryLandmarks();
			//convert to osg
			osg::Vec3d headPos(position.headCenter.x, position.headCenter.y, position.headCenter.z);
			osg::Vec3d headDeg(angles.yaw, angles.pitch, angles.roll);


			//headPoseFilter.add(headPos);
			//headPos = headPoseFilter.get();
			//headAngleFilter.add(headDeg);
			//headDeg = headAngleFilter.get();

			

			//Markers

			//Random plase in screen
			row = row++ % 800;
			osg::Vec2d screenPoint1(100, 150);// (row, 150);//
			cv::Point screenPoint(screenPoint1.x(), screenPoint1.y());

			//cv::Point screenPoint2(screenPoint1.x()+600, screenPoint1.y()+50);
			cv::Mat colorImage = cv::Mat(600, 800, CV_8UC3);
			colorImage = cv::Scalar::all(0);
			//if (firstTime)
			//{
				//firstTime = false;
				//cv::circle(colorImage, screenPoint, 5, cv::Scalar(0, 0, 255), -1);
				cv::circle(colorImage, screenPoint, 20, cv::Scalar(0, 0, 255), -1);
				//cv::circle(colorImage, screenPoint2, 20, cv::Scalar(0, 0, 255), -1);
				//cv::line(colorImage, screenPoint - cv::Point(2, 0), screenPoint + cv::Point(2, 0), cv::Scalar(255, 255, 255), 1);
				//cv::line(colorImage, screenPoint - cv::Point(0, 2), screenPoint + cv::Point(0, 2), cv::Scalar(255, 255, 255), 1);

			//}
			//{
				DrawHeadMarker(colorImage, headPos, headDeg);
			//}
			//convert to "world"
			osg::Vec3d world1 = pixelToWorld(screenPoint1);
			//call Sight
			pupilCenterFilterL.reset();
			pupilCenterFilterR.reset();

			if (Sight(colorImage, landmarkData, counter++, sample, headPos, headDeg, world1, screenPoint, "play1", aimLeft, aimRight))
			{
				//--------release the frame-------------------------
				
	/*			if (counter > 60)
				{*/
					//receive eye intersection
					//convert to pixel
					osg::Vec2d pixelL = worldToPixel(aimLeft);
					osg::Vec2d pixelR = worldToPixel(aimRight);
					//put on image
					cv::Point  centerL(pixelL.x(), pixelL.y());// screenPoint1.y());	//<<<-----debugOnly
					cv::Point  centerR(pixelR.x(), pixelR.y());// screenPoint1.y());
					cv::Point  center((pixelL.x() + pixelR.x()) / 2.0, (pixelL.y() + pixelR.y()) / 2.0);
					cv::circle(colorImage, centerL, 3, cv::Scalar(0, 255, 0), 1);
					cv::line(colorImage, cv::Point(centerL.x - 10, centerL.y), cv::Point(centerL.x + 10, centerL.y),  cv::Scalar(0, 255, 0), 2);
					cv::line(colorImage, cv::Point(centerL.x, centerL.y - 10), cv::Point(centerL.x, centerL.y + 10), cv::Scalar(0, 255, 0), 2);
					cv::circle(colorImage, centerR, 3, cv::Scalar(255, 0, 0), 1);
					cv::line(colorImage, cv::Point(centerR.x - 10, centerR.y), cv::Point(centerR.x + 10, centerR.y), cv::Scalar(255, 0, 0), 2);
					cv::line(colorImage, cv::Point(centerR.x, centerR.y - 10), cv::Point(centerR.x, centerR.y + 10), cv::Scalar(255, 0, 0), 2);
					cv::circle(colorImage, center, 3, cv::Scalar(255, 255, 255), 1);
					cv::line(colorImage, cv::Point(center.x - 10.
						, center.y), cv::Point(center.x + 10, center.y), cv::Scalar(255, 255, 255), 3);
					cv::line(colorImage, cv::Point(center.x, center.y - 10), cv::Point(center.x, center.y + 10), cv::Scalar(255, 255, 255), 3);
					//}
					{
						//drawEyes(sample->color, trackedFace);
						//drawDepthImage(sample->depth, true);
						//drawIrImage(sample->ir, true);
					}
					//Draw FeedBack
				cv::namedWindow("Pointing", CV_WINDOW_NORMAL);
				cv::setWindowProperty("Pointing", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
				cv::imshow("Pointing", colorImage);
				cv::waitKey(1);
				//char fileName[90];
				//sprintf(fileName, "%s_%d.jpg", name, counter);
				//std::string fN(fileName);
				//cv::imwrite(fN, colorImage);
				//video(colorImage);
			}
			senseManager->ReleaseFrame();
			if (kbhit())
			{
				int c = getch();
				if (c == 27){
					stop = true;
					char fileName[85];
					sprintf(fileName, "%s.jpg", name);
					std::string fN(fileName);
					cv::imwrite(fN, colorImage);
				}
			}
		}
	}
}


void BuildPath(int wx, int wy,int xJump , int yJump)
{

	int index = 0;
	bool inc = true;
	for (int row = 0; row < wy; row += yJump)
	{
		if (inc)
		{
			for (int col = 0; col < wx; col += xJump)
				ScreenPath[index++] = osg::Vec3d(col, row,0);
		}
		else
		{
			for (int col = wx - 1; col > -1; col -= xJump)
				ScreenPath[index++] = osg::Vec3d(col, row, 0);
		}
		inc = !inc;
	}
}
void BuildPathR(int wx, int wy,int cx, int cy, int rJump, int degJump)
{
	int maxRadius = min(wx - cx, wy - cy);
	int index = 0;
	bool inc = true;
	for (int i=0; i < 100; i++)
	{
		ScreenPath[index++] = osg::Vec3d(cx, cy, 0);
	}
	for (int radius = 0; radius < maxRadius; radius += rJump)
	{
		for (int deg = 0; deg < 360; deg += degJump)
		{
			int x = cx + radius * cos(osg::DegreesToRadians((double)deg));
			int y = cy + radius * sin(osg::DegreesToRadians((double)deg));
			ScreenPath[index++] = osg::Vec3d(x, y,deg);
		}

	}
}
void BuildPathCircle(int wx, int wy, int cx, int cy, int radius, int degJump,int times)
{
	int maxRadius = min(wx - cx, wy - cy);
	int index = 0;
	bool inc = true;
	for (int i = 0; i < 0; i++)
	{
		ScreenPath[index++] = osg::Vec3d(cx, cy,0);
	}
	int _radius = radius;
	for (int n = 0; n < times; n++)
	{
		for (int deg = 0; deg < 360; deg += degJump)
		{

			int x = cx + _radius * cos(osg::DegreesToRadians((double)deg));
			int y = cy + _radius * sin(osg::DegreesToRadians((double)deg));
			ScreenPath[index++] = osg::Vec3d(x, y,deg);
			Conversion* conv = new Conversion;
			conv->pixel.x() = x;
			conv->pixel.y() = y;
			conv->next = NULL;
			conv->useIt = false;
			angleResults[deg] = conv;
		}

	}
}
void BuildPathRectangle1(int wx, int wy, int cx, int cy,  int Jump)
{
	int index = 0;
	bool inc = true;
	double row = 0;
	double col = 0;
	int maxRadius = min(wx - cx, wy - cy);
	for (int i = 0; i < 100; i++)
	{
		ScreenPath[index++] = osg::Vec3d(cx, cy,0);
	}
	col = cx;
	row = cy;
	double increment = 1;
	for (int radius = 0; radius < maxRadius; radius += Jump)
	{
		increment = radius / 20.0;
	//	for (; col < cx + i* Jump; col++)
	//		ScreenPath[index++] = osg::Vec2d(col, row);
		for (int j = 0; j < radius; j += increment )
		{
			row+= increment;
			ScreenPath[index++] = osg::Vec3d(col, row,0);
		}
		for (int j = 0; j < radius; j += increment)
		{
			col+=increment;
			ScreenPath[index++] = osg::Vec3d(col, row,0);
		}

		for (int j = 0; j < 2 * radius; j += increment)
		{
			row -= increment;
			ScreenPath[index++] = osg::Vec3d(col, row,0);
		}
		for (int j = 0; j < 2 * radius; j += increment)
		{
			col -= increment;
			ScreenPath[index++] = osg::Vec3d(col, row,0);
		}
		for (int j = 0; j < radius; j += increment)
		{
			row += increment;
			ScreenPath[index++] = osg::Vec3d(col, row,0);
		}
		for (int j = 0; j < radius; j += increment)
		{
			col += increment;
			ScreenPath[index++] = osg::Vec3d(col, row,0);
		}
		int vgh = 7;
	}

}
void AddNode(int x, int y, osg::Vec2d left, osg::Vec2d right)
{
	Conversion p = table[x][y];
	Conversion* np = new Conversion();
	np->eyeL = left;
	np->eyeR = right;
	while (p.next != NULL)
		p = *(p.next);
	p.next = np;
}

void AddNode(std::map<double, Conversion*> &table ,double angle,int x, int y, osg::Vec2d left, osg::Vec2d right)
{
	Conversion* p = table[angle];
	Conversion* np = new Conversion();
	np->eyeL = left;
	np->eyeR = right;
	np->pixel.x() = x;
	np->pixel.y() = y;
	
	while (p->next != NULL)
		p = (p->next);
	p->next = np;
	
}
void BuildPathRectangle2(int wx, int wy, int times, int jump)
{
	int index = 0;
	bool inc = true;
	double row = 0;
	double col = 0;

	for (int i = 0; i < 100; i++)
	{
		ScreenPath[index++] = osg::Vec3d(wx/2, wy/2,0);
	}
	//col = cx;
	//row = cy;
	//double increment = 1;

	/*
	Conversion conv;
	conv.eyeL = leftPupil;
	conv.eyeR = rightPupil;
	table[0][tgtImagePos.x][tgtImagePos.y] = conv;
	*/
	for (int t = 0; t < times; t++)
	{
//		increment = radius / 20.0;

		for ( col = 0; col < wx; col += jump)
		{
			ScreenPath[index++] = osg::Vec3d(col, row,0);
		}
		for ( row = 0; row < wy; row += jump)
		{
			ScreenPath[index++] = osg::Vec3d(col, row,0);
		}

		for ( col = wx-1; col > -1; col -= jump)
		{
			ScreenPath[index++] = osg::Vec3d(col, row,0);
		}
		for ( row = wy-1; row > -1; row -= jump)
		{
			ScreenPath[index++] = osg::Vec3d(col, row,0);
		}
	}

}

FILE* f2d = NULL;
void play2D(char* name, PXCSenseManager* senseManager, PXCFaceData* m_output)
{
	if (f2d == NULL)
	{
		f2d = fopen("Eyes2d.txt", "w");
	}
	bool stop = false;
	int playTimes = 0;
	//----------------- define 	Black background -------------------------------
	cv::Mat colorImage = cv::Mat(600, 800, CV_8UC3);
	
	//BuildPath(800, 600, 5, 100);
	//BuildPathR(800, 600, 400, 300, 30, 5);
	//BuildPathRectangle2(800, 600,3,5);
	BuildPathCircle(800, 600, 400, 300, 200, 3, 5);
	colorImage = cv::Scalar::all(0);
	osg::Vec2d aimLeft, aimRight;
	//bool firstTime = true;
	printf("Press any key to continue");
	int a = getch();
	int counter = 0;
	int row = 0;
	int index = 0;
	headPoseFilter.reset();
	headAngleFilter.reset();
	for (; !stop;)  //for Aligned Images
	{
		//-----------------capture Image----------------------------
		pxcStatus sts = senseManager->AcquireFrame(true);//blocking
		if (sts < PXC_STATUS_NO_ERROR)
			break;
		PXCCapture::Sample *sample = senseManager->QuerySample();
		//-----------------prepare depth map on world coordinates------------------
		createVerticesFromDepth(sample->depth);
		m_output->Update();
		const int numFaces = m_output->QueryNumberOfDetectedFaces();
		//--------------if find face------------------


		if (numFaces > 0)
		{
			PXCFaceData::Face* trackedFace = m_output->QueryFaceByIndex(0);
			//--------------------------pose----------------------------
			const PXCFaceData::PoseData* poseData = trackedFace->QueryPose();
			PXCFaceData::PoseEulerAngles angles;
			PXCFaceData::HeadPosition position;
			poseData->QueryPoseAngles(&angles);
			poseData->QueryHeadPosition(&position);
			const PXCFaceData::LandmarksData* landmarkData = trackedFace->QueryLandmarks();
			//convert to osg
			osg::Vec3d headPos(position.headCenter.x, position.headCenter.y, position.headCenter.z);
			osg::Vec3d headDeg(angles.yaw, angles.pitch, angles.roll);





			//Markers

			//Random plase in screen
			row = row++ % 800;

			osg::Vec3d screenPoint1(750, 50,0);// (row, 150);//
			bool rawData = false;
			//if (counter < 100)
			
			if (ScreenPath.size() > index)
			{
				screenPoint1 = ScreenPath[index++];
				rawData = true;
			}
			else
				if (ScreenPath.size() > index)
				{

				}
			osg::Vec3d world1 = pixelToWorld(osg::Vec2d(screenPoint1.x(), screenPoint1.y()));
			cv::Point screenPoint(screenPoint1.x(), screenPoint1.y());

			cv::Point screenPoint2(screenPoint1.x() + 600, screenPoint1.y() + 50);
			cv::Mat colorImage = cv::Mat(600, 800, CV_8UC3);
			colorImage = cv::Scalar::all(0);

			//TODOD Draw something
			if (playTimes == 0)
				cv::circle(colorImage, screenPoint, 3, cv::Scalar(0, 0, 255), -1);
			else
				cv::circle(colorImage, screenPoint, 3, cv::Scalar(255, 255, 255), -1);

			//cv::circle(colorImage, screenPoint2, 20, cv::Scalar(0, 0, 255), -1);
			////cv::line(colorImage, screenPoint - cv::Point(2, 0), screenPoint + cv::Point(2, 0), cv::Scalar(255, 255, 255), 1);
			//DrawHeadMarker(colorImage, headPos, headDeg);
			//	
			////call Sight
			//pupilCenterFilterL.reset();
			//pupilCenterFilterR.reset();
			cv::Mat color = drawColorImage(sample->color, false);
			osg::Vec2d nose;
			bool res = true;
			if (res=EyeTrack(rawData,colorImage, landmarkData, counter++, sample, headPos, headDeg, screenPoint, "EyeTrack", aimLeft, aimRight,nose))
			{
				//--------release the frame-------------------------
				fprintf(f2d, "%lf %lf %lf %lf  %lf %lf \n", ScreenPath[index - 1].x(), ScreenPath[index-1].y(),aimLeft.x(), aimLeft.y(), aimRight.x(), aimRight.y());
				fflush(f2d);
				if (ScreenPath.size() >= index)
				{
					Conversion conv;
					conv.pixel.x() = ScreenPath[index - 1].x();
					conv.pixel.y() = ScreenPath[index - 1].y();
					conv.eyeL = aimLeft - nose;			
					conv.eyeR = aimRight - nose;
					ScreenConversion[index - 1] = conv;
					if (playTimes == 0)
					{
						AddNode(angleResults, ScreenPath[index - 1].z(), ScreenPath[index - 1].x(), ScreenPath[index - 1].y(), aimLeft - nose, aimRight - nose);
					}
				}
				if (ScreenPath.size() == index)
				{
					if (playTimes == 0)
					{
						//myPrint(angleResults);
						calculateAvg(angleResults);
						myPrintF(angleResults, "res.txt");
						index = 0;
						playTimes++;

					}
					else
					{
						out_capture.release();
						senseManager->ReleaseFrame();
						exit(1);
					}
				}
				///*			if (counter > 60)
				//{*/
				////receive eye intersection
				////convert to pixel
				//osg::Vec2d pixelL = aimLeft;
				//osg::Vec2d pixelR = aimRight;
				////put on image
				//cv::Point  centerL(pixelL.x(), pixelL.y());// screenPoint1.y());	//<<<-----debugOnly
				//cv::Point  centerR(pixelR.x(), pixelR.y());// screenPoint1.y());
				//cv::Point  center((pixelL.x() + pixelR.x()) / 2.0, (pixelL.y() + pixelR.y()) / 2.0);
				//cv::circle(colorImage, centerL, 3, cv::Scalar(0, 255, 0), 1);
				//cv::line(colorImage, cv::Point(centerL.x - 10, centerL.y), cv::Point(centerL.x + 10, centerL.y), cv::Scalar(0, 255, 0), 2);
				//cv::line(colorImage, cv::Point(centerL.x, centerL.y - 10), cv::Point(centerL.x, centerL.y + 10), cv::Scalar(0, 255, 0), 2);
				//cv::circle(colorImage, centerR, 3, cv::Scalar(255, 0, 0), 1);
				//cv::line(colorImage, cv::Point(centerR.x - 10, centerR.y), cv::Point(centerR.x + 10, centerR.y), cv::Scalar(255, 0, 0), 2);
				//cv::line(colorImage, cv::Point(centerR.x, centerR.y - 10), cv::Point(centerR.x, centerR.y + 10), cv::Scalar(255, 0, 0), 2);
				//cv::circle(colorImage, center, 3, cv::Scalar(255, 255, 255), 1);
				//cv::line(colorImage, cv::Point(center.x - 10.
				//	, center.y), cv::Point(center.x + 10, center.y), cv::Scalar(255, 255, 255), 3);
				//cv::line(colorImage, cv::Point(center.x, center.y - 10), cv::Point(center.x, center.y + 10), cv::Scalar(255, 255, 255), 3);
				////}
				{
					//drawEyes(sample->color, trackedFace);
					//drawDepthImage(sample->depth, true);
					//drawIrImage(sample->ir, true);
				}
				//Draw FeedBack
				//cv::namedWindow("Pointing", CV_WINDOW_AUTOSIZE  | CV_WINDOW_NORMAL);
				cv::namedWindow("Pointing", CV_WINDOW_NORMAL);
				cv::setWindowProperty("Pointing", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
				
				cv::imshow("Pointing", colorImage);
				cv::waitKey(1);
				cv::circle(color, cv::Point(aimLeft.x(), aimLeft.y()), 3, cv::Scalar(0, 255, 255), -2);
				char buffer[15];
				if (playTimes == 1)
				{
					osg::Vec2d pxL, pxR;
					findPoint(angleResults, aimLeft-nose, aimRight-nose, pxL, pxR);
					if (res)
						cv::circle(colorImage, cv::Point(pxL.x(), pxL.y()), 5, cv::Scalar(0, 255, 0), 2);
					//char buffer[15];
					sprintf(buffer, "TESTING");
					//CvFont font;
					//cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5);
					//cvPutText(&colorImage, "TESTING", cvPoint(2, 550), &font, cvScalar(255));
					//cv::putText(colorImage, buffer, cvPoint(2, 550), CV_FONT_NORMAL, 1, cv::Scalar(255, 255, 255), 1, 1);
				}
				else
				{
					//char buffer[15];
					sprintf(buffer, "TRAINNING");
					//CvFont font;
					//cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5);
					//cv::putText(colorImage, buffer, cvPoint(2, 550),CV_FONT_NORMAL, 1, cv::Scalar(255, 255, 255), 1, 1);
				}
				
				
				cv::resize(color, dstTemp, dstTemp.size());
				cv::cvtColor(dstTemp, dst, CV_BGRA2BGR);
				cv::flip(dst, dst, 1);
				cv::resize(colorImage, smalColor, smalColor.size());
				
				cv::putText(smalColor, buffer, cvPoint(2, 185), CV_FONT_HERSHEY_PLAIN, 0.50, cv::Scalar(0, 0, 255), 1, 1);	//CV_FONT_NORMAL	  //CV_FONT_HERSHEY_PLAIN



				smalColor.copyTo(dst.rowRange(0, 0+600/3).colRange(0, 0+800/3));
				
				//char fileName[85];
				//sprintf(fileName, "%s%d.jpg", "output",index);
				//std::string fN(fileName);
				//cv::imwrite(fN, dst);
				video(dst);

				//cv::Point left(AimL.x(), AimL.y());
				//cv::circle(colorImage, left, 2, cv::Scalar(255.0, 255.0, 0.0), 1);

				//char fileName[90];
				//sprintf(fileName, "%s_%d.jpg", name, counter);
				//std::string fN(fileName);
				//cv::imwrite(fN, colorImage);
				//video(colorImage);
			}
			senseManager->ReleaseFrame();
			//if (kbhit())
			//{
			//	int c = getch();
			//	if (c == 27){
			//		stop = true;
			//		char fileName[85];
			//		sprintf(fileName, "%s.jpg", name);
			//		std::string fN(fileName);
			//		cv::imwrite(fN, colorImage);
			//	}
			//}
		}
	}
}

//normals
void play3(char* name, PXCSenseManager* senseManager, PXCFaceData* m_output)
{
	bool stop = false;
	//----------------- define 	Black background -------------------------------
	cv::Mat colorImage = cv::Mat(600, 800, CV_8UC3);
	colorImage = cv::Scalar::all(0);
	osg::Vec3d aimLeft, aimRight;
	bool firstTime = true;
	printf("Press any key to continue");
	int a = getch();
	int counter = 0;
	int row = 0;
	for (; !stop;)  //for Aligned Images
	{
		//-----------------capture Image-------------------------b---
		pxcStatus sts = senseManager->AcquireFrame(true);//blocking
		if (sts < PXC_STATUS_NO_ERROR)
			break;
		PXCCapture::Sample *sample = senseManager->QuerySample();
		//-----------------prepare depth map on world coordinates------------------
		createVerticesFromDepth(sample->depth);
		m_output->Update();
		const int numFaces = m_output->QueryNumberOfDetectedFaces();
		//--------------if find face------------------


		if (numFaces > 0)
		{
			PXCFaceData::Face* trackedFace = m_output->QueryFaceByIndex(0);
			//--------------------------pose----------------------------
			const PXCFaceData::PoseData* poseData = trackedFace->QueryPose();
			PXCFaceData::PoseEulerAngles angles;
			PXCFaceData::HeadPosition position;
			poseData->QueryPoseAngles(&angles);
			poseData->QueryHeadPosition(&position);
			const PXCFaceData::LandmarksData* landmarkData = trackedFace->QueryLandmarks();
			//convert to osg
			osg::Vec3d headPos(position.headCenter.x, position.headCenter.y, position.headCenter.z);
			osg::Vec3d headDeg(angles.yaw, angles.pitch, angles.roll);


			headPoseFilter.add(headPos);
			headPos = headPoseFilter.get();
			headAngleFilter.add(headDeg);
			headDeg = headAngleFilter.get();

			//Random plase in screen
			row = row++ % 800;
			osg::Vec2d screenPoint1(100, 150);// (row, 150);//
			cv::Point screenPoint(screenPoint1.x(), screenPoint1.y());
			cv::Mat colorImage = cv::Mat(600, 800, CV_8UC3);
			colorImage = cv::Scalar::all(0);
			if (firstTime)
			{
				//firstTime = false;
				//cv::circle(colorImage, screenPoint, 5, cv::Scalar(0, 0, 255), -1);
				cv::circle(colorImage, screenPoint, 20, cv::Scalar(0, 0, 255), -1);
				cv::line(colorImage, screenPoint - cv::Point(2, 0), screenPoint + cv::Point(2, 0), cv::Scalar(255, 255, 255), 1);
				cv::line(colorImage, screenPoint - cv::Point(0, 2), screenPoint + cv::Point(0, 2), cv::Scalar(255, 255, 255), 1);

			}
			//convert to "world"
			osg::Vec3d world1 = pixelToWorld(screenPoint1);
			//call Sight
			pupilCenterFilterL.reset();
			pupilCenterFilterR.reset();

			if (SightWithNormal(colorImage, landmarkData, counter++, sample, headPos, headDeg, world1, screenPoint, "play1", aimLeft, aimRight))
			{
				//--------release the frame-------------------------

				if (counter > 60)
				{
					//receive eye intersection
					//convert to pixel
					osg::Vec2d pixelL = worldToPixel(aimLeft);
					osg::Vec2d pixelR = worldToPixel(aimRight);
					//put on image
					cv::Point  centerL(pixelL.x(), pixelL.y());// screenPoint1.y());	//<<<-----debugOnly
					cv::Point  centerR(pixelR.x(), pixelR.y());// screenPoint1.y());
					cv::Point  center((pixelL.x() + pixelR.x()) / 2.0, (pixelL.y() + pixelR.y()) / 2.0);
					cv::circle(colorImage, centerL, 1, cv::Scalar(0, 255, 0), 1);
					cv::circle(colorImage, centerR, 1, cv::Scalar(255, 0, 0), 1);
					//cv::circle(colorImage, center, 1, cv::Scalar(255, 255, 255), 1);
				}
				cv::namedWindow("Acurracy", CV_WINDOW_NORMAL);
				cv::setWindowProperty("Acurracy", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
				cv::imshow("Acurracy", colorImage);
				cv::waitKey(1);
			}
			senseManager->ReleaseFrame();
			if (kbhit())
			{
				int c = getch();
				if (c == 27){
					stop = true;
					char fileName[85];
					sprintf(fileName, "%s.jpg", name);
					std::string fN(fileName);
					cv::imwrite(fN, colorImage);
				}
			}
		}
	}
}
void SaveEyeCenter(char* name, osg::Vec3d leftEyeCenter, osg::Vec3d rightEyeCenter)
{
	double ZLavg = leftEyeCenter.z();
	double YLavg = leftEyeCenter.y();
	double XLavg = leftEyeCenter.x();
	double ZRavg = rightEyeCenter.z();
	double YRavg = rightEyeCenter.y();
	double XRavg = rightEyeCenter.x();



	FILE* fPersonal = fopen(name, "w");
	fprintf(fPersonal, "ZLavg %lf\n", ZLavg);
	fprintf(fPersonal, "YLavg %lf\n", YLavg);
	fprintf(fPersonal, "XLavg %lf\n", XLavg);
	fprintf(fPersonal, "ZRavg %lf\n", ZRavg);
	fprintf(fPersonal, "YRavg %lf\n", YRavg);
	fprintf(fPersonal, "XRavg %lf\n", XRavg);
	fflush(fPersonal);
	fclose(fPersonal);
}
void GetDC(std::map<int, osg::Vec3d> left, std::map<int, osg::Vec3d> right, osg::Vec3d& dl, osg::Vec3d& dr)
{
	double ZLavg = 0;
	double YLavg = 0;
	double XLavg = 0;
	double ZRavg = 0;
	double YRavg = 0;
	double XRavg = 0;
	for each (auto pos in left)
	{
		ZLavg += pos.second.z();
		XLavg += pos.second.x();
		YLavg += pos.second.y();
	}
	ZLavg /= left.size();
	YLavg /= left.size();
	XLavg /= left.size();
	dl = osg::Vec3d(XLavg, YLavg, ZLavg);


	for each (auto pos in right)
	{
		ZRavg += pos.second.z();
		YRavg += pos.second.y();
		XRavg += pos.second.x();
	}
	ZRavg /= right.size();
	YRavg /= right.size();
	XRavg /= right.size();
	dr = osg::Vec3d(XRavg, YRavg, ZRavg);


}
void anglesCalibration(char* name,PXCSenseManager* senseManager, PXCFaceData* m_output)
{
	int task = -2;
	bool stop = false;
	static int counter2 = 0;
	osg::Vec3d	idealAim, aimL, aimR;
	int centerProcCounter = 0;
	osg::Vec3d leftCenter, rightCenter,tl,tr,tl2,tr2;
	for (; !stop;)  //for Aligned Images
	{

		pxcStatus sts = senseManager->AcquireFrame(true);//blocking
		if (sts < PXC_STATUS_NO_ERROR)
			break;
		PXCCapture::Sample *sample = senseManager->QuerySample();
		//......

		cv::Mat colorImage = cv::Mat(600, 800, CV_8UC3);
		colorImage = cv::Scalar::all(255);
		
		//drawIrImage(sample->ir, true);
		//drawDepthImage(sample->depth,true);

		createVerticesFromDepth(sample->depth);

		m_output->Update();
		const int numFaces = m_output->QueryNumberOfDetectedFaces();
		

		if (numFaces > 0)
		{
			PXCFaceData::Face* trackedFace = m_output->QueryFaceByIndex(0);
			//--------------------------pose----------------------------
			const PXCFaceData::PoseData* poseData = trackedFace->QueryPose();
			PXCFaceData::PoseEulerAngles angles;
			PXCFaceData::HeadPosition position;
			poseData->QueryPoseAngles(&angles);	
			poseData->QueryHeadPosition(&position);
			//angles
			// yaw + to right
			//pitch + is up
			osg::Vec3d headPos(position.headCenter.x, position.headCenter.y, position.headCenter.z);
			osg::Vec3d headDeg(angles.yaw, angles.pitch, angles.roll);
			headAngleFilter.add(headDeg);
			headDeg = headAngleFilter.get();
			headPoseFilter.add(headPos);
			headPos = headPoseFilter.get();

			
			printf("yaw: %lf\n", headDeg.x());
			osg::Matrixd _headToWorld, _worldTohead;
			_worldTohead = Utilities::worldToHead(headPos, headDeg);
			_headToWorld = Utilities::headToWorld(headPos, headDeg);

			

			cv::Size size = colorImage.size();
	
			cv::Point p1(size.width / 2, 230);
			cv::Point p2(size.width / 2, 350);
			cv::Point p3(150+size.width / 2, 230);
			cv::Point p4(150+size.width / 2, 350);
			cv::Point centerY(10.0*headDeg.x() + size.width / 2, 250);
			cv::Point centerR(10.0*headDeg.y() + size.width / 2, 250);
			cv::Point centerP(10.0*headDeg.z() + size.width / 2, 250);
			cv::Rect xH(-headPos.x()  + size.width / 2, 250 + 40, 20, 20);
			//printf("Head Angle(%f,%f)\n", angles.yaw, angles.roll);
			cv::line(colorImage, p1, p2, cv::Scalar(0, 0, 255), 1);
			cv::line(colorImage, p3, p4, cv::Scalar(0, 0, 255), 1);
			cv::circle(colorImage, centerY, 10, cv::Scalar(0, 255, 255), 1);
			cv::circle(colorImage, centerR, 10, cv::Scalar(0, 255, 128), 1);
			cv::circle(colorImage, centerP, 10, cv::Scalar(255, 0, 128), 1);
			cv::rectangle(colorImage, xH, cv::Scalar(255, 0, 0), 1);
			const PXCFaceData::LandmarksData* landmarkData = trackedFace->QueryLandmarks();
			cv::Point  tgt0Center(400, 5);
			cv::Point  tgtCenter(400, -43);
			osg::Vec3d tgt(0, 0, 0);
			osg::Vec3d tgt0 = pixelToWorld(osg::Vec2d(tgt0Center.x, tgt0Center.y));// (0, -40.0 - 01.25, -10.0);
			cv::Point tgt1Center(5, 5);
			osg::Vec3d tgt1 = pixelToWorld(osg::Vec2d(tgt1Center.x, tgt1Center.y));// (172.5 - 2.5, -40.0 - 01.25, -10.0);
			cv::Point tgt2Center(795, 5);
			osg::Vec3d tgt2 = pixelToWorld(osg::Vec2d(tgt2Center.x, tgt2Center.y));// (-172.5 + 2.5, -40.0 - 01.25, -10.0);
			cv::Point tgt3Center(5, 300);
			osg::Vec3d tgt3 = pixelToWorld(osg::Vec2d(tgt3Center.x, tgt3Center.y)); //(172.5 - 2.5, -40.0 - 01.25 - (192.5 / 2.0), -10.0);
			cv::Point  tgt4Center(400, 300);
			osg::Vec3d tgt4 = pixelToWorld(osg::Vec2d(tgt4Center.x, tgt4Center.y));// (0, -40.0 - 01.25 - (192.5 / 2.0), -10.0);
			cv::Point tgt5Center(795, 300);
			osg::Vec3d tgt5 = pixelToWorld(osg::Vec2d(tgt5Center.x, tgt5Center.y)); //(-172.5 + 2.5, -40.0 - 01.25 - (192.5 / 2.0), -10.0);
			cv::Point tgt8Center(795, 595);
			osg::Vec3d tgt8 = pixelToWorld(osg::Vec2d(tgt8Center.x, tgt8Center.y));// (-172.5 + 2.5, -40.0 - 01.25 - (192.5 / 1.0), -10.0);
			cv::Point tgt6Center(5, 595);
			osg::Vec3d tgt6 = pixelToWorld(osg::Vec2d(tgt6Center.x, tgt6Center.y)); //(172.5 - 2.5, -40.0 - 01.25 - (192.5 / 1.0), -10.0);
			cv::Point  tgt7Center(400, 595);
			osg::Vec3d tgt7 = pixelToWorld(osg::Vec2d(tgt7Center.x, tgt7Center.y));// (0, -40.0 - 01.25 - (192.5 / 1.0), -10.0);
			osg::Vec3d pupilLeft;
			osg::Vec3d pupilLeftOnHead;
			osg::Vec3d pupilRight;
			osg::Vec3d pupilLeftAngle;
			osg::Vec3d pupilRightAngle;
			osg::Vec3d eyeLid_LL, eyeLid_LR, eyeLid_LB, eyeLid_LT;
			osg::Vec3d pupilCenterLeft;
			osg::Vec3d leftEyeCenter;
			osg::Vec3d eyeAngle;
			double rx, ry, rz;
			osg::Matrixd rMatrix;
			osg::Vec3d calculatedAngelLeft;
			osg::Vec2d pixel;
			bool calibratePoint;
			
			//build3DModel(landmarkData, sample, headPos, headDeg);
			switch (task)
			{
				case -2:
					cv::putText(colorImage, "Press any key to start.", cv::Point(200, 160), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					break;
				case -1 :
					cv::putText(colorImage, "See forward into the camera.", cv::Point(200, 160), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					break;
				case 0:
					if (centerProcCounter++ < 100)
					{
						//https://software.intel.com/en-us/blogs/2015/01/26/can-your-webcam-do-this
						if (!AnotherCalibration(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt, tgtCenter, "Right tgt0", idealAim, aimR))
						{
							centerProcCounter--;
						}

						//GetEyeCenter(landmarkData, sample, tl, tr);
						//osg::Matrixd t1 = getRotmatrix(headDeg);
						//osg::Matrixd t1I;
						//osg::Matrixd t2,t2I;
						//t2.makeTranslate(-headPos.x(), -headPos.y(), -headPos.z());
						//t2I.invert(t2);
						//t1I.invert(t1);
						//tl2 = tl*t2;
						//tr2 = tr * t2 ;
						//tl2 = tl2*t1I;
						//tr2 = tr2*t1I;
						//tl = tl*_worldTohead;
						//tr = tr* _worldTohead;
						//leftCenter += tl;
						//rightCenter += tr;
					}else
					{
						SaveEyeCenter("tmp1.txt", LeftCenterFixMap, RightCenterFixMap);
						LeftCenterFixMap.clear();
						RightCenterFixMap.clear();
						//for each (auto pos in LeftCenterFixMap)
						//{
						//	ZLavg += pos.second.z();
						//	YLavg += pos.second.y();
						//}
						//ZLavg /= LeftCenterFixMap.size();
						//YLavg /= LeftCenterFixMap.size();

						//for each (auto pos in RightCenterFixMap)
						//{
						//	ZRavg += pos.second.z();
						//	YRavg += pos.second.y();
						//}
						//ZRavg /= RightCenterFixMap.size();
						//YRavg /= RightCenterFixMap.size();

						//double ZLvar = 0;
						//for each (auto pos in LeftCenterFixMap)
						//{
						//	ZLvar += (pos.second.z() - ZLavg)*(pos.second.z() - ZLavg);
						//}
						//ZLvar /= LeftCenterFixMap.size();
						//double ZRvar = 0;
						//for each (auto pos in RightCenterFixMap)
						//{
						//	ZRvar += (pos.second.z() - ZRavg)*(pos.second.z() - ZRavg);
						//}
						//ZRvar /= RightCenterFixMap.size();
						//leftCenter /= centerProcCounter;
						//rightCenter /= centerProcCounter;
						task++;
					}
					break;
				case 1:
					cv::putText(colorImage, "Put your head On Center ", cv::Point(200, 100), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					cv::putText(colorImage, "Each time look at the the red CIRCLE ", cv::Point(200, 130), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					cv::putText(colorImage, "Do not move and Press any key .", cv::Point(200, 160), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					pupilFilterL.reset();
					pupilFilterR.reset();
					cv::circle(colorImage, tgt0Center, 5, cv::Scalar(0, 0, 255), -1);
					centerProcCounter = 0;
					break;
				case 2:
					counter2++;
					//PointCalibration2(colorImage, landmarkData, counter2, sample, true, leftCenter, headPos, headDeg, tgt0, tgt0Center, "Left tgt0", idealAim, aimL);
					//if (PointCalibration2(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt0, tgt0Center, "Right tgt0", aimL, aimR))
					//	PointCalibration2(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt0, tgt0Center, "Right tgt0", aimL, aimR);
					if (centerProcCounter++ < 100)
					{
						if (!AnotherCalibration(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt0, tgt0Center, "Right tgt0", aimL, aimR))
							centerProcCounter--;
					}
					else
					{
						task++;
						SaveEyeCenter("tmp2.txt",LeftCenterFixMap, RightCenterFixMap);
						LeftCenterFixMap.clear();
						RightCenterFixMap.clear();
					}
					//aimL.y() = aimR.y() = -45.0;
					//aimL.x() = aimR.x() = 0.0;

					break;
				case 3:
					cv::putText(colorImage, "Put your head On Center ", cv::Point(200, 100), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					cv::putText(colorImage, "Each time look at the the red CIRCLE ", cv::Point(200, 130), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					cv::putText(colorImage, "Do not move and Press any key .", cv::Point(200, 160), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					counter2 = 0;
					pupilFilterL.reset();
					pupilFilterR.reset();
					cv::circle(colorImage, tgt1Center, 5, cv::Scalar(0, 0, 255), -1);
					centerProcCounter = 0;
					break;
				case 4:				
					counter2++;				
					//PointCalibration(colorImage, landmarkData, counter2, sample, true, leftCenter, headPos, headDeg, tgt1, tgt1Center, "Left tgt1", idealAim, aimL);
					//if (PointCalibration2(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt1, tgt1Center, "Right tgt1", aimL, aimR))
					////if (AnotherCalibrationLU(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt0, tgt0Center, "Right tgt0", idealAim, aimR))
					//{
					if (centerProcCounter++ < 100)
					{
						if (!AnotherCalibration(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt1, tgt1Center, "Right tgt1", aimL, aimR))
							centerProcCounter--;
					}
					else
					{
						task++;
						SaveEyeCenter("tmp3.txt", LeftCenterFixMap, RightCenterFixMap);
						LeftCenterFixMap.clear();
						RightCenterFixMap.clear();
					}
					
					
					//}
					//aimL.y() = aimR.y() = -45.0;
					//aimL.x() = aimR.x() = 165.0;
					break;
				case 5:
					cv::putText(colorImage, "Put your head On Center ", cv::Point(200, 100), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					cv::putText(colorImage, "Each time look at the the red CIRCLE ", cv::Point(200, 130), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					cv::putText(colorImage, "Do not move and Press any key .", cv::Point(200, 160), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					counter2 = 0;
					pupilFilterL.reset();
					pupilFilterR.reset();
					cv::circle(colorImage, tgt2Center, 5, cv::Scalar(0, 0, 255), -1);
					centerProcCounter = 0;
					break;
				case 6:
					counter2++;
					//PointCalibration(colorImage, landmarkData, counter2, sample, true, leftCenter, headPos, headDeg, tgt2, tgt2Center, "Left tgt2", idealAim, aimL);
					//if (PointCalibration2(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt2, tgt2Center, "Right tgt2", aimL, aimR))
					if (centerProcCounter++ < 100)
					{
						if (!AnotherCalibration(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt2, tgt2Center, "Right tgt2", aimL, aimR))
							centerProcCounter--;
					}
					else
					{
						task++;
						SaveEyeCenter("tmp4.txt", LeftCenterFixMap, RightCenterFixMap);
						LeftCenterFixMap.clear();
						RightCenterFixMap.clear();
					}
						
					//	task++;
					//aimL.y() = aimR.y() = -45.0;
					aimL.x() = aimR.x() = -165.0;
					break;
				case 7:
					cv::putText(colorImage, "Put your head On Center ", cv::Point(200, 100), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					cv::putText(colorImage, "Each time look at the the red CIRCLE ", cv::Point(200, 130), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					cv::putText(colorImage, "Do not move and Press any key .", cv::Point(200, 160), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					counter2 = 0;
					pupilFilterL.reset();
					pupilFilterR.reset();
					cv::circle(colorImage, tgt3Center, 5, cv::Scalar(0, 0, 255), -1);
					centerProcCounter = 0;
					break;
				case 8:
					counter2++;
					//PointCalibration(colorImage, landmarkData, counter2, sample, true, leftCenter,headPos, headDeg, tgt3, tgt3Center, "Left tgt3", idealAim, aimL);
					//if (PointCalibration(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt3, tgt3Center, "Right tgt3", idealAim, aimR))
					if (centerProcCounter++ < 100)
					{
						if (!AnotherCalibration(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt3, tgt3Center, "Right tgt3", aimL, aimR))
							centerProcCounter--;
					}
					else
					{
						task++;
						SaveEyeCenter("tmp5.txt", LeftCenterFixMap, RightCenterFixMap);
						LeftCenterFixMap.clear();
						RightCenterFixMap.clear();
					}
					break;				
				case 9:
						cv::putText(colorImage, "Put your head On Center ", cv::Point(200, 100), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
						cv::putText(colorImage, "Each time look at the the red CIRCLE ", cv::Point(200, 130), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
						cv::putText(colorImage, "Do not move and Press any key .", cv::Point(200, 160), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
						counter2 = 0;
						pupilFilterL.reset();
						pupilFilterR.reset();
						cv::circle(colorImage, tgt4Center, 5, cv::Scalar(0, 0, 255), -1);
						centerProcCounter = 0;
						break;
				case 10:
						counter2++;
						//PointCalibration(colorImage, landmarkData, counter2, sample, true, leftCenter, headPos, headDeg, tgt4, tgt4Center, "Left tgt4", idealAim, aimL);
						//if (PointCalibration(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt4, tgt4Center, "Right tgt4", idealAim, aimR))
						if (centerProcCounter++ < 100)
						{
							if (!AnotherCalibration(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt4, tgt4Center, "Right tgt4", aimL, aimR))
								centerProcCounter--;
						}
						else
						{
							task++;
							SaveEyeCenter("tmp6.txt", LeftCenterFixMap, RightCenterFixMap);
							LeftCenterFixMap.clear();
							RightCenterFixMap.clear();
						}
						break;				
				case 11:
						cv::putText(colorImage, "Put your head On Center ", cv::Point(200, 100), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
						cv::putText(colorImage, "Each time look at the the red CIRCLE ", cv::Point(200, 130), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
						cv::putText(colorImage, "Do not move and Press any key .", cv::Point(200, 160), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
						counter2 = 0;
						pupilFilterL.reset();
						pupilFilterR.reset();
						cv::circle(colorImage, tgt5Center, 5, cv::Scalar(0, 0, 255), -1);
						centerProcCounter = 0;
						break;

				case 12:
						counter2++;
						//PointCalibration(colorImage, landmarkData, counter2, sample, true, leftCenter, headPos, headDeg, tgt5, tgt5Center, "Left tgt5", idealAim, aimL);
						//if (PointCalibration(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt5, tgt5Center, "Right tgt5", idealAim, aimR))
						if (centerProcCounter++ < 100)
						{
							if (!AnotherCalibration(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt5, tgt5Center, "Right tgt5", aimL, aimR))
								centerProcCounter--;
						}
						else
						{
							task++;
							SaveEyeCenter("tmp7.txt", LeftCenterFixMap, RightCenterFixMap);
							LeftCenterFixMap.clear();
							RightCenterFixMap.clear();
						}

						break;
				case 13:
					cv::putText(colorImage, "Put your head On Center ", cv::Point(200, 100), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					cv::putText(colorImage, "Each time look at the the red CIRCLE ", cv::Point(200, 130), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					cv::putText(colorImage, "Do not move and Press any key .", cv::Point(200, 160), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					counter2 = 0;
					pupilFilterL.reset();
					pupilFilterR.reset();
					cv::circle(colorImage, tgt6Center, 5, cv::Scalar(0, 0, 255), -1);
					centerProcCounter = 0;
					break;

				case 14:
					counter2++;
					//PointCalibration(colorImage, landmarkData, counter2, sample, true, leftCenter, headPos, headDeg, tgt6, tgt6Center, "Left tgt6", idealAim, aimL);
					//if (PointCalibration(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt6, tgt6Center, "Right tgt6", idealAim, aimR))
					if (centerProcCounter++ < 100)
					{
						if (!AnotherCalibration(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt6, tgt6Center, "Right tgt6", aimL, aimR))
							centerProcCounter--;
					}
					else
					{
						task++;
						SaveEyeCenter("tmp8.txt", LeftCenterFixMap, RightCenterFixMap);
						LeftCenterFixMap.clear();
						RightCenterFixMap.clear();
					}
					break;
				case 15:
					cv::putText(colorImage, "Put your head On Center ", cv::Point(200, 100), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					cv::putText(colorImage, "Each time look at the the red CIRCLE ", cv::Point(200, 130), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					cv::putText(colorImage, "Do not move and Press any key .", cv::Point(200, 160), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					counter2 = 0;
					pupilFilterL.reset();
					pupilFilterR.reset();
					cv::circle(colorImage, tgt7Center, 5, cv::Scalar(0, 0, 255), -1);
					centerProcCounter = 0;
					break;

				case 16:
					counter2++;
					//PointCalibration(colorImage, landmarkData, counter2, sample, true, leftCenter, headPos, headDeg, tgt7, tgt7Center, "Left tgt7", idealAim, aimL);
					//if (PointCalibration(colorImage, landmarkData, counter2, sample, false, headPos, rightCenter, headDeg, tgt7, tgt7Center, "Right tgt7", idealAim, aimR))
					if (centerProcCounter++ < 100)
					{
						if (!AnotherCalibration(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt7, tgt7Center, "Right tgt7", aimL, aimR))
							centerProcCounter--;
					}
					else
					{
						task++;
						SaveEyeCenter("tmp9.txt", LeftCenterFixMap, RightCenterFixMap);
						LeftCenterFixMap.clear();
						RightCenterFixMap.clear();
					}
					break;
				case 17:
					cv::putText(colorImage, "Put your head On Center ", cv::Point(200, 100), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					cv::putText(colorImage, "Each time look at the the red CIRCLE ", cv::Point(200, 130), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					cv::putText(colorImage, "Do not move and Press any key .", cv::Point(200, 160), CV_FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0, 0, 0), 1);
					counter2 = 0;
					pupilFilterL.reset();
					pupilFilterR.reset();
					cv::circle(colorImage, tgt8Center, 5, cv::Scalar(0, 0, 255), -1);
					centerProcCounter = 0;
					break;

				case 18:
					counter2++;
					//PointCalibration(colorImage, landmarkData, counter2, sample, true, leftCenter, headPos, headDeg, tgt8, tgt8Center, "Left tgt8", idealAim, aimL);
					//if (PointCalibration(colorImage, landmarkData, counter2, sample, false, rightCenter,headPos, headDeg, tgt8, tgt8Center, "Right tgt8", idealAim, aimR))
					if (centerProcCounter++ < 100)
					{
						if (!AnotherCalibration(colorImage, landmarkData, counter2, sample, false, rightCenter, headPos, headDeg, tgt8, tgt8Center, "Right tgt8", aimL, aimR))
							centerProcCounter--;
					}
					else
					{
						task++;
						SaveEyeCenter("tmp10.txt", LeftCenterFixMap, RightCenterFixMap);
						LeftCenterFixMap.clear();
						RightCenterFixMap.clear();
					}
					break;
				case 19:
					{
					SaveEyeCenter(name, LCenterFixMap, RCenterFixMap);
					SaveLos();
					}

					task++;
					break;
				case 20:
					stop = true;
					break;
			}
			//pixel =worldToPixel(idealAim);
			//cv::Rect idealRect(pixel.x(), pixel.y(), 30, 30);
			//cv::rectangle(colorImage, idealRect, cv::Scalar(0, 0, 255), 1);
			pixel = worldToPixel(aimL);
			cv::Rect aimRect(pixel.x(), pixel.y() , 25, 25);
			cv::rectangle(colorImage, aimRect, cv::Scalar(0, 255, 0), 1);
			pixel = worldToPixel(aimR);
			cv::Rect aimRectR(pixel.x(), pixel.y(), 25, 25);
			cv::rectangle(colorImage, aimRectR, cv::Scalar(64, 128, 0), 1);

		}
		
		cv::namedWindow("Angles Calibration", CV_WINDOW_NORMAL);
		cv::setWindowProperty("Angles Calibration", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
		cv::imshow("Angles Calibration", colorImage);
		cv::waitKey(1);
		senseManager->ReleaseFrame();
		if (kbhit())
		{
			char c = getchar();
			task++;
		}
			
	}

}





void calibration(PXCSenseManager* senseManager, PXCFaceData* m_output)
{
	char name[80];
	char fname[85];
	//headCalibration(senseManager, m_output);

		printf("Your name:");
		scanf("%s", name);
		sprintf(fname, "%s.txt", name);
		anglesCalibration(fname,senseManager, m_output);
		//anglesCalibrationByPixels(name, senseManager, m_output);
		//anglesCalibration2(fname,senseManager, m_output);
		//normals
		//anglesCalibration3(fname, senseManager, m_output);
	exit(0);

}
int main(int argc, char** argv)
{
	osg::ArgumentParser arguments(&argc, argv);

	//CalculateNormalTest();
	//TestHough();
	//TestCenter();
	//TestPixelsMethod();

	fFile = fopen("bANgles.txt", "w");
	PXCFaceData* m_output;
	g_session = PXCSession::CreateInstance();
	bool stop = false;
//C:\projects\Dependencies\OpenSceneGraph - 3.0.1\data
	scene = osgDB::readNodeFile("C:/projects/GazeTracking/Code/GazeTracking/GazeTracking.flt");
//	scene = osgDB::readNodeFile("C:/projects/Dependencies/OpenSceneGraph - 3.0.1/data/cessna.osg");

	PopulateDevice();
	//PopulateStreams();
	PXCSenseManager* senseManager = g_session->CreateSenseManager();
	PXCCapture::DeviceInfo info;
	g_device->QueryDeviceInfo(&info);
	captureManager = senseManager->QueryCaptureManager();
	if (senseManager == NULL)
	{
		//todo //FaceTrackingUtilities::SetStatus(dialogWindow, L"Failed to create an SDK SenseManager", statusPart);
		return 1;
	}
	
	senseManager->EnableStream(PXCCapture::STREAM_TYPE_COLOR, 1920, 1080, 30);
	senseManager->EnableStream(PXCCapture::STREAM_TYPE_DEPTH,640,480,30);
	senseManager->EnableStream(PXCCapture::STREAM_TYPE_IR, 640, 480, 30);// PXCImage::PIXEL_FORMAT_RGB32, 0, 0);
	//g_device->SetMirrorMode(PXCCapture::Device::MIRROR_MODE_HORIZONTAL);
	//-----face-----
	senseManager->EnableFace();
	PXCFaceModule* faceModule = senseManager->QueryFace();
	PXCFaceConfiguration* config = faceModule->CreateActiveConfiguration();
	
	config->landmarks.isEnabled = true;
	config->pose.isEnabled = true;												   
	config->ApplyChanges();
	
	//----------------------------
	projection = g_device->CreateProjection();
	createVertexImage(640, 480);
	senseManager->Init();
		m_output = faceModule->CreateOutput();
	//printf("Need calibration (Y/N)\n");
	char name[80];
	char fname[85];
	std::string _name;
	//char c = getchar();
	char c = 'n';
	if (c == 'Y' || c == 'y')
	{
		//eyeCalibration("",senseManager, m_output);
		
		calibration(senseManager, m_output);
	}
	else
	{

		//headCalibration(senseManager, m_output);

		//printf("Your name:");
		//scanf("%s", name);
		sprintf(name, "default");
		//sprintf(name, "xx3");
		_name = std::string(name);
		sprintf(fname, "%s.txt", name);
		FILE* fPersonal = fopen(fname, "r");
		while (fPersonal == NULL)
		{
			printf("File not found try again\n");
			printf("Your name:");
			scanf("%s", name);
			sprintf(fname, "%s.txt", name);
			fPersonal = fopen(fname, "r");
		}
		
		fscanf(fPersonal, "%s %lf", name, &ZLavg);
		fscanf(fPersonal, "%s %lf", name, &YLavg);
		fscanf(fPersonal, "%s %lf", name, &XLavg);
		fscanf(fPersonal, "%s %lf", name, &ZRavg);
		fscanf(fPersonal, "%s %lf", name, &YRavg);
		fscanf(fPersonal, "%s %lf", name, &XRavg);
		
		fclose(fPersonal);
		

	}
	c = 'd';
	c = 'c';
	while (c != 'n'&&c != 'N' && c != 'C' && c != 'c')
	{
		printf("Play Normals or by center (N/C)\n");
		c = getche();
	}
	if (c == 'N' || c == 'n')
	{
		play3(name, senseManager, m_output);
	}
	else
	{
		play2D(fname, senseManager, m_output);
		//play1(fname, senseManager, m_output);
		//ReadCalibratedPixelsMethod((char*)_name.c_str());
		//playbyPixel(fname, senseManager, m_output);
	}
	return 0;

}

